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ABSTRACT 

The  technique  of  Extended  Kalman  filtering  is  applied 
to  a  passive  single  sensor  location  and  tracking  problem. 
Signal  angle  of  arrival  and  doppler  shifted  frequency  are 
used  as  observations  for  two  different  filter  formulations: 
one  for  fixed  X-Y  coordinates  of  position  and  velocity  and 
one  with  an  orthogonal  system  rotated  to  align  an  axis  with 
the  target  heading.   The  fixed  X-Y  system  was  found  to  give 
better  tracking  performance  on  the  Monte  Carlo  simulation. 
A  new  method  of  calculating  the  initial  covariance  matrix 
was  extended  to  reformulate  the  Kalman  filter  equations  for 
a  nonlinear  problem.   The  use  of  the  full  initial  covariance 
matrix  as  opposed  to  only  the  diagonal  elements,  provides 
better  transient  response  and  lower  error  variances  in  the 
simulation. 
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I.   INTRODUCTION 

There  are  many  engineering  problems  in  nonlinear  filter- 
ing which  do  not  have  the  luxury  of  many  observations  over 
an  extended  period  of  time.   The  estimator  or  filter  is 
required  to  provide  a  rapid,  accurate  solution  before  steady 
state  conditions  of  the  gains  and  covariance  of  error  are 
reached.   In  these  situations  it  is  imperative  to  use  as 
much  information  as  possible  from  the  observations,  the 
known  apriori  conditions,  and  the  statistical  relationship 
of  the  initial  filter  states  . 

The  location  of  acoustic  emitters  by  remote  passive 
sensors  is  such  a  problem.   The  received  acoustic  signals 
may  be  processed  to  obtain  noisy  angle  of  arrival  and 
noisy  doppler  shifted  frequency  data.   These,  in  turn,  can 
be  used  as  observables  to  provide  location  and  tracking 
information  on  the  emitting  target.   The  ability  to  accom- 
plish this  with  a  single  sensor  (sonobuoy)  as  well  as  the 
flexibility  to  incorporate  additional  sensors  as  available 
would  be  highly  desirable  for  the  Anti-Submarine  Warfare 
Community.   A  filter  using  angle  of  arrival  and  doppler- 
shifted  frequency  can  be  applied  to  other  tracking  problems 
in  the  radar  domain  as  well. 

There  are  presently  available  methods  which  use  only 
the  doppler-shifted  frequency  from  some  array  of  three  or 
more  sensors  [^6].   In  general,  they  require  a  good  initial 
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guess  as  to  where  the  emitter  is  located  because  they  rely 
on  a  linearization  method  closely  resembling  a  least-square 
fit  of  the  received  frequency  data.   Currently,  research 
is  going  on  to  incorporate  angle  and  frequency  data  in  a 
common  arrangement  [35]. 

A  first  objective  of  this  research  was  to  apply  non- 
linear filtering  theory  to  the  single  sensor  location  and 
tracking  problem  and  develop  an  algorithm  to  solve  this 
estimation  problem.   The  calculation  of  initial  filter 
states  using  a  single  sensor  with  angle  and  frequency  measure- 
ments only,  represents  a  difficult  task.   A  single  sensor 
making  either  measurement  alone  does  not  provide  a  unique 
solution  and  it  is  only  when  the  combination  is  used  that 
a  single  determination  of  the  target  position  is  possible. 
The  geometry  of  the  problem,  while  simple,  does  not  make 
determination  of  initial  position  using  the  specified  obser- 
vations  very  simple.   Yet,  this  initial  position  estimate 
must  be  chosen  so  that  the  subsequent  linearized  filter 
techniques  will  converge.   In  doing  so,  the  effect  of  various 
choices  which  are  available  to  the  filter  designer  became 
evident.   The  primary  choice  is  that  of  a  system  model, 
and  even  for  a  simple  constant-velocity  target,  differences 
in  the  model  formulation  are  possible.   The  value  of  the 
initial  filter  states  and  their  resultant  uncertainty,  as 
reflected  in  the  initial  covariance  matrix,  are  seen  to 
play  important  rolos  in  the  time  it  takes  for  the  filter 
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to  converge.   Using  all  of  the  covariance  terms  In  this 
matrix,  as  opposed  to  only  the  diagonal  variances,  results 
in  better  filter  performance  in  the  transient  phase  and 
lower  error  variances  for  a  specified  track  duration.   The 
geometric  placement  of  sensors  was  considered  to  determine 
where  they  might  be  placed  for  best  filter  performance. 
It  is  also  important  to  determine  how  well  the  filter  is 
performing  while  in  operation,  not  so  much  as  a  quantitative 
measure  of  accuracy,  but  whether  the  filter  is  indeed  esti- 
mating a  reasonable  track.   Following  the  lead  of  Periot 
[3^],  the  filter  residuals  were  found  to  be  a  good  measure 
of  a  valid  track. 

In  Chapter  Two  general  filtering  theory  is  considered 
with  the  emphasis  on  the  underlying  probability  structure 
of  the  optimal  filter.   This  approach  clearly  points  out 
the  simplicity  of  a  linear-Gaussian  system  and  its  resultant 
filtering  solution  as  well  as  the  difficulties  and  complexi- 
ties of  the  nonlinear  problem.   The  continuous  formulation 
of  the  filtering  problem  Is  also  reviewed  with  reference 
to  stochastic  integrals  and  the  determination  of  the  strictly 
formal  solution.   In  the  final  section  of  Chapter  Two,  the 
techniques  that  various  authors  have  used  to  approximate 
the  optimal  nonlinear  solution  will  be  presented.   The 
approaches  range  from  a  direct  calculation  of  the  aposteriori 
density  function  [10],  to  a  simple  linearization  of  state 
and  observation  equations  about  some  nominal  state  trajectory  [3?. 
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Chapter  Three  describes  in  detail  the  single  sensor 
passive  location  and  tracking  problem  and  will  develop  the 
two  state  formulations.   The  equations  for  an  Extended 
Kalman  Filter  for  each  case  are  presented.   The  important 
initial  condition  equations  are  derived  and  the  initial 
covariance  of  error  expressions  for  the  states  are  found. 
The  technique  used  to  obtain  the  Initial  covariance  matrix 
is  new  and  consists  of  a  direct  calculation  of  the  changes 
in  the  nonlinear  initialization  equations  when  the  input 
measurements  change  by  one  standard  deviation.   The  values 
for  the  filter  state  excitation  matrix  (which  turn  out  to  be 
state  dependent) s    are  presented  next.   The  ability  to  add 
additional  sensors  or  observables  to  the  filter  is  an 
important  advantage  of  the  Extended  Kalman  method  and  this 
is  developed  next. 

Chapter  Pour  contains  a  new  approach  to  the  determina- 
tion of  error  covariances  for  nonlinear  functions  of  random 
variables.   This  approach  is  an  extension  of  the  method 
used  to  obtain  the  initial  covariance  of  state  error  matrix. 
The  method  may  be  readily  applied  to  formulate  the  complete 
nonlinear  filtering  problem  using  the  linear  Kalman  filter 
equations.   The  one  step  prediction  and  updating  is  accom- 
plished by  a  direct  calculation  of  the  change  in  the  non- 
linear state  or  observation  function  caused  by  a  one-standard- 
deviation  change  in  each  state  variable.   The  matrix  of  the 
correlation  coefficients  of  the  states  is  found  to  play  an 
important  role . 
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A  Monte  Carlo  simulation  of  the  two  filters  developed 
in  Chapter  Three  is  given  in  Chapter  Five  for  a  wide  range 
of  problem  geometry.   The  effects  of  initial  conditions 
and  initial  covariance  matrices  will  be  seen  to  play  an 
important  role  in  performance  of  the  filter. 

The  final  chapter  summarizes  the  results  of  this  inves- 
tigation and  presents  the  conclusions  and  suggestions  for 
further  study. 
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II.   FILTERING  THEORY 

A.   THE  PROBABILISTIC  APPROACH  FOR  THE  DISCRETE  CASE 

Since  the  extraction  of  states  or  parameters  from  noisy 
measurements  is  fundamentally  a  problem  of  estimating  random 
variables,  it  is  Important  to  consider  the  underlying  prob- 
ability theory  of  a  general  filtering  problem.   Indeed  the 
characterization  of  the  estimates  with  the  appropriate  prob- 
ability density  function  is  the  most  complete  knowledge  that 
can  be  obtained  about  these  estimates.   The  evolution  of  this 
density  function  in  time,  and  its  change  with  incoming  measure- 
ments, represents  the  general  solution  to  the  estimation  prob- 
lem.  How  the  value  for  the  estimate  is  determined  will  be 
governed  by  the  cost  or  penalty  charged  for  making  a  wrong 
choice  [31»W. 

Assume  a  discrete  system  of  the  following  form 

x(k+l)  =  f(x(k),k)  +  g_(x(k),k)-w(k)  (2.1) 

z(k)  =  h(x(k),k)  +  v(k)  (2.2) 

with  x  the  vector-  of  states  or  parameters  (or  both)  to  be 
estimated  and  z_  the  vector  of  observations.   The  notation  used 
throughout  the  text  is  that  vector  quantities  are  lower  case 
letters  and  underlined  and  matrices  are  given  by  upper  case 
letters.   The  initial  states  x(0)  a^e  assumed  to  be  from  some 
initial  density  p(x(0))  and  the  statistics  of  the  system  and 
measurement  noise  w  and  v  are  known  and  given  by 


21 


E[w(k)]  =  0 

E[v(k)]  =  0 

E[w(k)-v(j)]  =  0      for  all  k,j 

E[£(x(k),k)-w(k).(g(x(k),k).w(k))T]  =  Q(k)5kj 

E[v(k)-vT(j)]  =  R(k)-6kJ 


with 


5kJ  =  X    k=J 

=  0    k*J   , 

and 

E[w(k)-xT(0)]  =  0 

E[v(k).xT(0)]  =  0      .  (2.3) 


Note  that  the  above  system  of  equations  defines  a  Markov 
process;  that  is,  a  process  whose  probability  law  for  future 
times  depends  only  on  the  present  state  values.  In  terms  of 
the  density  functions 

p(x(k+l)/x(k),x(k-l)  ...  x(0))  =  p(x(k+l)/x(k))   (2.4) 

The  Markov  property  of  a  process  is  very  fundamental  and 
along  with  the  uncorrelated  noise  assumptions  written  above, 
allows  great  simplification  in  the  recursive  expressions 
developed  later. 
Let 

Zk  =  {z(k),z(k-l),. . .,z(l)}  (2.5) 

be  the  set  of  all  measurements  on  the  system  up  to  the 
present  time.   The  probabilistic  knowledge  one  has  about  the 
system,  given  the  set  of  measurements  Z  s    is  contained  in  the 
conditional  probability  density  function  of  the  states  given 
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the  measurements:   p(x(j)/Z  ).   Note  that  the  Index  value 
j  and  superscript  k  are  not  necessarily  the  same.   This 
allows  the  three  main  classes  of  problems  —  smoothing  for 
j  <  k,  filtering  for  j  =  k,  and  prediction  for  j  >  k  —  to 
be  considered  simultaneously. 
1.   The  Cost  Function 

Let  the  error  of  the  estimate  be-e(j)  =  x(j)  ~  *(S) 
where  x(j )  is  the  true  state  value  at  time  t(j)  and  x( J )  is 
the  value  of  the  estimate.   Let  L(e(j))  be  a  cost  function. 
The  value  of  the  estimate  is  chosen  so  as  to  minimize  the 
expected  value  of  the  cost  function: 

/\ 

x( j )  is  the  value  of  x(j)  such  that  E[L(e(j))]  is  minimum, 

with 

E[L(e(j))]  =  /  L(e(j))p(x(j)/Zk)  dx( j )   . 

This  expectation  is  taken  over  the  conditional  density 
function  of  the  states  and  therein  lies  the  importance  of 
this  function. 

Table  I  gives  three  main  cost  functions  which  lead 
to  different  choices  for  the  estimate. 
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Cost  Function  L(e(j))     Name  Estimate  x(j) 


1.  e  (j)*e(j)  minimum  variance   Conditional 

or  minimum  mean   mean 
square  error 


2.  |e_(j)|  minimum  absolute  Conditional 

error  median 


3.  =  0   e  ( j )  <  6_         uniform  Conditional  mode 

~  or  maximum 

a  posteriori 
estimate 


=  1   otherwise 


TABLE  I.   Cost  Functions  and  Their  Estimates 


2.   Recursive  Bayesian  Formulation 

In  the  discrete  formulation  of  the  filtering  problem, 
the  conditional  density  function  can  be  written  in  a  recursive 
manner  with  the  initial  condition  probability  distribution 
as  the  starting  function  [26]. 

Let  the  conditional  density  of  the  filter  states  be 
p(x(k)/Zk).   Using  Bayes  rule  this  can  be  written  as: 

.       p(x(k),  z(k),  Zk_1) 

p(x(k)/Zk)  =  p(x(k)/z(k),  ZK  x)  =  r-rr 

"  p(z(k)  ,  zk  x) 


p(z(k)/x(k),  Zk  1)     •    p(x(k),  Zk  1) 


p(z(k)/Zk_1)-  p(Zk  J) 

.  p(z(k)/x(k))  ,, 

p(x(k)/Zk)  =  — = W  •  P^2i(k)/Zk  2)  (2.7) 

p(z(k)/ZR_J) 


2'l 


Thus  the  current  value  of  the  conditional  density  function 
is  found  from  the  predicted  density  p(x(k)  /  Z    )  and  a 
weighting  term  which  depends  on  the  current  observation. 
Since  the  denominator  term  does  not  depend  on  the  states, 
x(k),  it  can  be  looked  on  as  a  normalizing  constant[10] . 

The  predicted  conditional  density  function  can  be 
found  from  the  density  function  one  step  back  in  the  following 
manner: 


p(x(k)  /  Zk-1)=   /     p(x(k),  x(k-D  /  Zk_1) 

x(k-l) 


V-,  p(x(k),x(k-l),Zk  X) 

p(x(k)/ZK  x)    =        f  j— i dx(k-l) 

x(k-l)         p(Zk_1) 


/ 


p(x(k)/x(k-l),Zk  1)    p(x(k-l)5Zk  1)  dx(k_±) 


x(k-l  p(Z    )  (2.8) 


Now  using  the  Markov  property  of  the  state  system. 


p(x(k)/Zk  1)  =   /     p(x(k)/x(k-l))  p(x(k-l)/Zk  1)    dx(k-l) 

x(k-l) 

(2.9) 


The  notation  for  the  integral  indicates  that  the  integration 
is  performed  over  all  permissible  values  o'f  the  states  one 
step  back.   The  predicted  conditional  density  is  thus  found 
from  the  density  one  step  back  and  involves  a  spatial 
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Integration  over  all  values  of  all  the  states  at  that  time 
The  final  recursive  expression  for  the  conditional  density 
is  thus 


p(x(k)/Zk)  = 


p(z(k)/x(k))  k  , 

/     p(x(k)/x(k-l)p(x(k-l)/ZK  -1)  dx(k-l) 


(z(k)/Zk_1    x(k-l) 


(2.10) 


with  the  condition  p(x(0)/Z°)=  p(x(0)).   The  individual 
conditional  density  functions  above  are  obtained  from  the 
probability  density  functions  of  the  state  excitation  noise, 
w(k-l),  and  the  observation  noise,  v(k)  . 

Let  the  density  of  w(k)  be  p(w(k))  and  the  density 
of  v(k)  be  p  (v(k).   Therefore 

p(z(k))/x(k))  =  Pv(z(k)  -  h(x(k),k))  ,  (2.11) 

and 

p(x(k)/x(k-l))  =  pM[£"1(x(k-l),k-l)-[x(k)-f(x(k-l),k-l)]] 

(2.12) 


3.   Linear  Gaussian  Case 

When  the  state  equations  and  the  observation 
equations  are  linear,  and  the  initial  density  of  x(0)  and 
the  densities  of  w(k-l)  and  v(k)  are  Gaussian  all  of  the 
above  conditional  densities  turn  out  to  be  Gaussian  since 
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they  involve  linear  operations  on  Gaussianly  distributed  . 
random  variables.   Since  these  densities  are  completely 
determined  by  two  quantities,  the  vector  mean  and  the 
covariance  matrix,  the  recursive  expression  for  the  condi- 
tional density  function,  Eq .  (2.10)  simplifies  to  two 
coupled  expressions:   one  for  the  vector  mean  and  the  other 
for  the  covariance  matrix.   These  two  expressions,  when 
rewritten  are  the  well-known  Kalman  filter  equations  [36, 
22,23]. 

When  the  state  or  observation  equations  are  nonlinear 
or  the  noise  statistics  are  non-Gaussian,  the  above  simpli- 
fication is  not  valid.   Thus  the  entire  density  is  needed 
to  completely  solve  the  problem. 

B.   THE  CONTINUOUS  CASE 

The  equations  for  the  states  and  observations  can  be 
written  in  continuous  form 

dx(t)  =  f(x(t),t)dt  +  g_(x(t),t)dw(t),        (2.13) 


and 


dz(t)  =  h(x(t),t)dt  +  dv(t)  .  (2.14) 

The  above  equations  are  Ito  stochastic  differential 
equations  and  must  be  manipulated  using  the  rules  of  Ito 
calculus  [17].   The  difficulty  with  ordinary  calculus  is  in 
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the  definition  of  integrals  involving  white  noise.   The 
first  equation  above  is  to  be  interpreted  as 

t  t 

■x(t)  -  x(t  )  =   /  f(x(t),t)dt  +   /  g(x(t),t)dw(t). 

0  (2.15) 

Since  the  last  integral  involves  integration  over  random 
Increments  (dw(t)),  with  random  quantities  in  the  integral, 
it  is  not  even  defined  by  ordinary  calculus  [21].   The 
increments  appearing  in  Equations  (2.13)  and  (2.14),  dw(t) 
and  dv(t)  are  increments  of  two  independent  Wiener  processes 

When  the  continuous  equations  are  interpreted  in  the 
Ito  sense,  the  process  x(t)  still  defines  a  Markov  process 
and  as  such,  evolution  of  the  density  function  of  x(t)  is 
given  by  the  Kolmogorov  equations  or  the  Fokker-Planck 
equation  [19, ^5].   Kushner  [25],  Bucy  [6],  and  Fisher  [18] 
have  shown  that  the  conditional  density  P(x(t)/Z  )  -  where 
Z   is  the  set  of  observations  in  the  interval  (t  ,t)  -  also 
obeys  a  Kolmogorov  type  equation  which  is  modified  by  the 
observations.   The  result  is  an  Ito  differential  equation  . 
for  the  conditional  density  which  is  strictly  a  formal 
solution  to  the  problem.   By  this  is  meant  that  no  closed 
form  solution  can  be  found  because  there  is  no  theory  to 
solve  the  equation  [21].   See  also  Buoy  and  Joseph  [8]. 

C.   SOLUTION  MET?IODS  AND  APPROXIMATIONS 

It  Is  no  surprise,  and  this  work  is  no  exception,  that 
most  of  the  practical  non-linear  filters  use  the  Kalman 
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filter  equations  in  some  linearized  system  of  equations 
[38].   The  linearization  is  usually  carried  out  about  some 
nominal  state  trajectory  or  about  a  predicted  value  for  the 
states  on  a  step  by  step  basis.   This  technique  is  Extended 
Kalman  filtering.   The  tacit  assumption  here  is  that  with 
Gaussian  noise,  there  is  some  region  in  state  space  around 
the  true  state  values  -where  the  system  response  is  linear 
and  therefore  Gaussian.   Similarly  the  observation  errors 
will  be  linear  and  Gaussian.   The  greatest  difficulty  with 
this  approach  is  insuring  that  the  linearization  is  valid, 
especially  at  the  initial  filter  estimates  where  the  errors 
may  be  quite  large.   For  the  passive  problem  described  in 
the  next  chapter,  the  initial  conditions  are  not  known  and 
must  be  obtained  from  the  first  few  observations  with  suffi- 
cient accuracy  to  insure  filter  convergence. 

Several  authors  have  looked  at  higher  order  terms  in 
the  expansions  of  the  state  and  observations  equations 
[3,5,27,40,43].   This  greatly  Increases  the  complexity  of 
the  calculations  and  has  not  been  shown  to  be  generally 
applicable  to  all  problems. 

There  have  been  attempts  to  look  at  higher  moments  of 
the  conditional  density  function  itself  in  an  effort  to 
characterize  the  function  at  each  state  point.   Fisher  [18] 
has  derived  equations  for  the  evolution  of  higher  order 
moments.   The  difficulty  is  in  determining  how  many  moments 
are  needed  to  describe  the  density  and  in  solving  the  highly 
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coupled  set  of  nonlinear  difference  or  differential  equations 
which  are  the  result  [24]. 

Srinivasan  [4l]  and  Sorensen  and  Stubberuds  [39]  expanded 
the  density  into  an  othogonal  series  and  then  looked  at  the 
evolution  of  each  of  the  terms  in  the  expansion.   It  is 
difficult  to  know  how  many  terms  to  incorporate  and  in  some 
cases  the  series  may  not  converge  at  all  [21].   Spline 
function  expansions  have  been  used  but  have  the  same  inherent 
difficulties  [14].   Swirling,  et  al .  [42]  have  attempted  a 
logarithmic  expansion  of  the  conditional  mean. 

One  of  the  most  successful  expansion  techniques  has 
been  the  Gaussian-Sum  Expansion  of  Alspach  and  Sorenson 
[1,2].   While  the  computational  burden  is  quite  large, 
this  method  is  one  of  the  few  that  will  solve  multi-modal 
distributions.   The  primary  reason  for  its  universal  appli- 
cation is  that  each  linearization  of  the  state  and  observa- 
tion equation  only  has  to  be  valid  about  the  mean  of  each 
of  the  assumed  Gaussian  distributions  used  in  the  sum. 
Each  term  in  the  sum  is  then  filtered  using  the  linear 
Kalman  filter  equations,  and  the  results  recombined  and 
normalized  to  give  an  approximation  to  the  conditional 
density. 

For  limited  order  systems,  Bucy  and  Senne  [7]  have  done 
a  direct  calculation  of  the  conditional  density  using  the 
Bayes  law  recursive  equations  developed  in  the  previous  sec- 
tion.  Here  again  the  computational  burden  is  enormous  and 
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is  only  possible  with  two  or  three  state  variables.   For 
these  systems,  however,  this  technique  .does  result  in  the 
complete  solution  to  the  nonlinear  filtering  problem  in  the 
sense  that  the  result  is  the  complete  aposteriori  condi- 
tional density  function.   The  particular  value  of  the  state 
estimate  may  then  be  determined  using  a  suitable  cost  cri- 
terion.  See  also  Senne  [37]  and  Bucy,  Hecht  and  Senne  [9]. 

The  method  of  least  squares,  while  being  strictly  a 
non-probabilistic  approach  is  widely  used  in  non-linear 
problems  [4,12,20,28,33].   In  this  method  a  non-probabilistic 
cost  function  is  developed  which  is  usually  a  quadratic  form 
of  the  state  trajectory  with  some  appropriate  state  model 
(See  for  example  Sage  and  Melsa  [36]).   This  cost  function 
is  then  minimized,  using  the  observed  data,  with  respect 
to  the  state  trajectory  points.   As  new  data  is  added  a 
recursive  algorithm  can  be  used  to  update  the  estimates 
[15,30].   With  appropriate  weighting  matrices  used  in  the 
cost  function,  this  method  can  be  shown  to  be  exactly  equi- 
valent for  Gaussian  noise  statistics  to  the  maximum  a  pos- 
teriori estimate  using  a  probability  approach  [21,36]. 

The  method  of  least  squares  for  nonlinear  problems  is 
usually  solved  by  linearization  about  some  estimate  and 
then  iteration  until  a  solution  is  found.   Thus  the  initial 
guess  must  be  sufficiently  close  to  allow  for  convergence. 
Also  for  a  large-order  system  the  amount  of  computation  may 
be  extremely  large. 
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The  technique  Involves  setting  up  a  suitable  cost 
function  as  a  set  of  simultaneous  non-linear  equations 
which  contains  the  observed  data  and  some  set  of  state 
parameters  as  unknowns.   In  this  system  of  equations 
(usually  over-determined)  the  "best  fit"  criteria  is  applied, 
generally  with  a  data  error  norm.   This  method  is  used  in 
the  doppler-frequency-only  localisation  method  [46].   When 
the  system  of  over-determined  equations  is  linear,  this 
approach  results  in  the  pseudo-inverse  matrix  technique 
[28]. 
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III.   PASSIVE  LOCATION  AND  TRACKING 

In  the  two-dimensional  location  problem,  there  are  two 
position  and  two  velocity  components  which  will  specify  a 
constant-speed,  constant-heading  target.   Since  the  dcppler 
shifted  frequency  is  used  as  an  observable,  it  is  also 
necessary  to  estimate  the  rest  frequency  of  the  emitter. 
These  five  quantities  will  be  used  to  define  two  different 
filter  formulations:  -one  in  which  a  target  independent  X-Y 
coordinate  frame  is  used  and  one  in  which  a  target  dependent 
system  with  one  axis  aligned  with  the  estimated  heading  is 
used.   Before  presenting  the  two  filter  types,  a  general 
discussion  of  the  Extended  Kalman  filter  is  given. 

A.   EXTENDED  KALMAN  FILTER 

Consider  a  nonlinear  discrete  system  of  state  and 
observation  equations  given  by 

x(k+l)  =  f(x(k)sk)  +  g(x(k),k)-w(k)         (3-D 
and 

z(k)  =  h(x(k),k)  +  v(k)  .  (3-2) 

In  these  equations  f,  g_  and  h  are  nonlinear  functions  of 
the  state  variables  x,  w(k)  is  plant  excitation  noise,  and 
v(k)  is  measurement  noise.   The  plant  noise  and  measurement 
noise  are  assumed  uncorrelated, zero-mean,  and  white.   That 
is 
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E[w(k)  •  wx(j)]  =  Q'(k)  6, 


and 


EEv(k).  vT(j)]  =  R(k)   6 


*j 


kj 


In  order  to  apply  the  linear  filter  equations,  (3.1) 
and  (3.2)  are  expanded  about  the  best  estimate  of  the  state 
at  that  time  and  only  the  first-order  terms  are  kept. 
Equation  (3.1)  gives 


x(k+l)  =  $(k)x(k)  +  T(k)  •  w(k)   , 


with 


Kk)  = 


9f 
^x 


x=x(k) 


(3.3) 


(\M 


Similarly  Eq .  (3.2)  yields 


z(k)  =  H(k)  x(k)  +  v(k) 


(3.4) 


where 


9h 

H(k)  =  -„— 
3x 


x=x'  (k) 


(3.4a) 


A  th 

x(k)  is  the  estimated  state  value  after  the  k   measurement 
and  x' (k)  is  the  predicted  value  of  the  state  before  the 
k   measurement.   That  is, 


x' (k)  -  f (x(k-l) ,k-] .) 


3^ 


A  state  error  vector  is  defined  by 

x(k)  =  x(k)  -  x(k) 
and  a  predicted  state  error  vector  is  defined  by 

x» (k)  =  x' (k)  -  x(k)   . 

The  covariance  of  state  error  matrix  is  defined  by 

P(k)  =  E[x(k)  •  xT(k)] 

and  the  predicted  covariance  of  state  error  matrix  is 
given  by 

P» (k)  =  E[x'(k)  •  x,T(k)] 


The  state  excitation  matrix  is  given  by 


a 


Q(k)  =  E[r(k|-w(k)  •  wT(k)-rT(k)]   , 


L_ 


J 


and  the  measurement  noise  covariance  matrix  is 


R(k)  =  E[v(k)  •  v'Ck)] 
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The  Kalman  Filter  equations  are  given  by  [38] 


•hP'(k+l)  ='  <p(k)P(k)$1(k)  +  Q(k)  (3. 5a) 

G(k)  =  P«(k)HT(k)[H(k)P'(k)HT(k)  +  R(k)]"1       (3. 5b) 


P(k)  =  [I  -  G(k)H(k)]  P'(k)  (3.5c) 

x'(k)  =  f(£(k-l),k)  (3-5d) 

z' (k)  =  h(x' (k),k)  (3.5e) 

x(k)  =  x'(k)  +  G(k)[z(k)  -  z'(k)]  .           (3-5f) 


The  Q  matrix  serves  not  only  to  allow  for  maneuvering 
but  also  to  account  for  any  model  inaccuracies.   That  Is, 
any  discrepancies  between  the  true  action  of  the  physical 
system  and  its  characterization  by  Eq.  (3-3).   For  a  filter 
which  reaches  steady-state  conditions  the  Q  also  serves  to 
prevent  the  gain  matrix  G(k)  from  approaching  zero  by  always 
insuring  uncertainty  in  the  predicted  covariance  of  error 
matrix  P' (k) . 

1 .   X-Y  Filter  Equations 

Figure  3.1  gives  the  geometry  of  the  states  used  for 
the  X-Y  filter.   For  a  constant-course,  constant-speed  target, 
the  plant  state  equations  can  be  described  as  two  second  order 
systems,  one  for  X  and  one  for  Y,  and  a  state  for  the  rest 
frequency,  F  .   These  are 
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Target  (>;,  y) 
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vy 


Fig.  3-1     x-y  Filter  Geometry 
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x(k+l)  = 


x(k+l) 

vx(k+l) 

y(k+l) 

vy(k+l) 

P0(k+1) 

<  4. 

x(k)+Tvx(k)+f1(y^  ,y^  ,k) 


s   s 


vx(k)+f2(Y.  ,Y.  ,k) 
s   s 


y(k)+Tv  (k)+f.+y.  5y.  ,k) 
y     3   e  '    v  ' 
s   s 


Vk)+fVYe  >xv  >k) 

s    s 


P  (k)+f~(YA  ) 
o 


(3.6) 


where  x(k),  and  y(k)  are  the  position  coordinates  at  time 

t(k),  v  (k)  and  v  (k)  are  the  X  and  Y  components  of  velocity 

T  is  the  time  between  observation,  i.e.,  t(k+l)  -  t(k),  and 

F  (k)  is  the  rest  frequency  of  the  emitter. 

The  excitation  terms  f-,  through  fp-  are  included  to 

account  for  the  random  changes  in  speed  and  heading  and  F 

which  can  occur  for  a  maneuvering  target.   The  quantities 

YA  j  Y,%  and  Y-&   are  the  random  changes  of  the  target.   They 

s     s       o 
are  assumed  to  be  independent,  zero  mean,  piecewise-constant 

rates  of  change.   They  have  variances  defined  by 


2  -  cr    2i 
a •    =  E  Ly* 
v       '  v 
s        s 


°6  2  ■  E[^e  2] 
s        s 


a   2  =  E[Y*  2] 
o        o 
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The  values  for  the  standard  deviations  were  taken  from 
typical  maneuvering  parameters  for  the  target; 


c-      =  100°/1000  sec 
y 

5? 


,olls<" 


&* 


r 


o-      =  10  kts/1000  sec 
Vs 


as,  =  .5  Hz/1000  sec 
o 


,wo'  '' 


t«- 


The  effect  of  this  excitation  is  to  increase  the  predicted 
value  of  the  covariance  of  the  state  error  matrix. 

Writing  the  equations  in  linear  state  form  results 


in 


.  ^ 


(k+1)  =  $  x(k)  +  r  w(k) 


(3.7) 


where 


and 
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The  vector  w(k)  represents  the  effect  on  the  states  of  the 
random  excitations  and  may  be  calculated  from  the  equations 
relating  target  X  and  Y  velocity  to  the  target  heading  and 
velocity.   The  X  velocity  is 


x  =  Vx  =  Vcos  es 


(3.8) 


which  when  differentiated  gives 


x  =  y  • cos  0 
vs     s 


Similarly 


y  =  v„  =  v  •  sin  6   } 
y    s      s   J 


(3.10) 


and 


"   VV 

y  =  ^L.y^    +  Vx.Y. 

s   s        s 


(3.11) 


The  frequency  term  is  just 


Fo  =  YF 


(3-12) 


Thus 


v 


w(k)T  =  [  o   Y.  .  -2.  ..  Y.  .  Vy  o  Y;_.  ^  +  T5_-  vx  yf 


v 
_2 

vs 


o 
(3-13) 


v/here  from  the  assumption  on  the  y's 


E[w(k)]  =  0 


(3.1*0 


14  0 


The  excitation  co-variance  matrix  is  thus  found  from 


Let 


Q  =  E[r  w(k)  w(k)T  rT] 


6?c     Y'f£ 


^(H  \£(& 


}rr 


^ 


a»2  =  (Ix)2  a.  2  +   2    2 

V    vs     y    8s 


eTW  (3.15) 


(3.16a) 


.-2 


v 


=  (-^)2  a-  ^  +  v/  a; 


v 


(3.16b) 


and 


r  °Vs  '      2  n 

a""  =  vxVy[  ~ T  "  a9    ] 
xy         v^      s 


(3.16c) 


where  the  states  are  evaluated  at  the  current  state  estimates 5 
x(k).   Substituting  these  expressions  in  the  Q  matrix  results 
in 


Q  = 


2?- 


<ttK 


T3  r„2 
—   ax 


2-2 


symmetric 


T2 
(~)axy  Y  CTxy 


T" 


T 


3 


,  a""   T2a"" 
2   xy     xy 


,T2s2  ..2  73   ..2 
(-)  cy   -r  ay 


T2c"2 

y 


o 
o 


T  a, 


o 


(3-17) 


The  excitation  matrix  serves  not  only  to  take  into 
account  the  possibility  of  maneuvering,  but  of  model 
inaccuracies  as  well.   Q  is  also  used  to  prevent  the  gains 


of  the  filter  from  approaching  zero  as  more  and  more  data 
is  processed,  by  insuring  some  uncertainty  in  the  predicted 
state  values.   (See  Sage  and  Melsa  [36],  page  415.) 

The  observation  equations  are  nonlinear  in  the  states 
and  are  given  by 


z(k)  = 


e(k) 


f(k) 


tan 


~1 


y(k) 
x(k) 


+  v0(k) 


P0(k)  vp 


v  +  vscos(es(k)-6(k)) 


+  v.(k) 


(3.18a) 


(3.18b) 


The  doppler  equation  can  be  expressed  in  terms  of  the  state 
variables  to  give 


f(k)  = 


P0(k)  vp 


x(k)  vx(k)  +  y(k)  vy(k) 


+  vf(k) 


(3-19) 


Vp    (x(k)2  +  y(k)2)172 


The  measurement  noises,  v0(k)  and  vf(k),  are  assumed  to  be 
zero-mean  and  independent  with  a  covariance  matrix 


R(k)  ■- 


'f 


(3-20) 


The  magnitude  of  the  angle  noise  is  a  function  of 
the  processor  used  and  the  signal-to-noise  ratio  at  the 


H2 


sensor.   A  typical  accuracy  of  ±  five  degrees  is  common  for 
strong  signals  and  this  value  was  used  as  the  standard 
deviation  for  most  of  the  simulation  runs. 

The  resolution  of  the  frequency  measurements  is 
equal  to  the  inverse  of  the  record  length  of  the  time 
signal.   For  a  25-second  record  this  would  be  .04  Hz  and 
this  value  was  used  throughout  the  simulation  as  the  frequency 
noise  standard  deviation.   This  value  would  represent  a  one- 
bin  uncertainty  in  the  coefficients  of  a  fast  Fourier 
transform  processor  which  typically  could  compute  the  trans- 
form in  less  than  100  ms . 

Equation  (3.4a)  can  be  used  to  give  the  linearized 
observation  matrix.   The  result  is 


H(k)  = 


9G(k)     38(k)     30(k)     36(k)     98(k) 


3x 


9f(k) 
3x 


3v. 


3f(k) 
*vx 


3y 


9f(k) 

ay 


3v. 


3f(k) 

9vy 


3F. 


o 


3f(k) 

3F_ 


When  the  derivatives  are  taken  and  evaluated  at  the 
predicted  state  values  x' (k)  the  result  is 
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H(k)  = 


y'(k) 
x'(k)2+y'(k) 


2 


p;(k)Vp 


y' (k)[y » (k)v' (k)-x' (k)v'(k)] 

*  -  d 

[x'(k)2+y'(k)2]3/2 


f*(k)' 


F'(k)vp 


x»(k) 

[x'(k)2+y'(k)2]1/2 


x'(k) 


x'(k)2+y'(k)2 


f'(k) 

F;(k)Vp 


x' (k)[x' (k)v^(k)-y'(k)v;(k)! 

[x'(k)2+y'(k)2]3/2 


f " 00* 

F' (k)v 
o   p 


y»(k) 
[x'WVtk)2]17' 


f '  (k) 

F'lkT 


(3-22) 


The  $  matrix,  Q  matrix,  R  matrix,  and  H  matrix  are 
then  used  in  the  Kalman  filter  equations  (3.13). 

In  the  X-Y  filter,  both  position  coordinates  will 
change  as  functions  of  time.   Furthermore,  a  change  in 
either  heading  or  speed  will  result  In  a  change  in  both 


'I 'I 


velocity  components.   Thus,  the  calculation  of  the  state 

excitation  matrix,  Q,  involves  all  state  related  terms 

except  for  those  involving  P  .   Since  a  constant-speed, 

constant-heading  target  is  assumed,  a  different  set  of  states 

can  be  found  which  will  decouple  the  heading  and  speed 

excitation  and  involve  the  estimation  of  only  one  linearly, 

time-varying  quantity. 

Let  R  „  be  the  range  to  the  target  at  the  closest 
cpa  to  & 

point  of  approach  (cpa),  X    be  the  distance  to  cpa  (negative 

cp  a 

before  —  positive  after),  v_  be  target  speed,  0   be  target 
heading  and  F  be  target  rest  frequency.   The  geometry  is 
shown  in  Pig.  3.2.   Expressed  in  this  coordinate  system  the 
state  equations  are 


Rcpa(k+1)  =  Rcpa(k)  +  *L(YS  ,Y«  ,k) 


Xcpa(k+1)  =  XcPa(k)  +  VT  +  M^e  'Y-  »k) 

s    s 


v  (k+1)  =  v  (k)  +  g,(y-  )  (3.23) 

S  S  3        Vg 


es(k+i)  ■  es(k)  +  gjjCyg  ) 

P  (k+1 3  =  F0(k)  +  gr(Yp  ) 

o 


with  g.  through  gr  the  random  forcing  terms. 
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Fig.  3.2   R    „  -  X^„     Filter  Geometry 
3  cpa         cpa 
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The  state  transition  matrix  is  therefore 


*  = 


0 

0 

0 

0 

1 

n 

J. 

0    . 

0 

1 

0 

0 

1 

0 

1 

The  angle  observation  equation  is 


A-R  „„(k) 


6(k)  =  6  fk)  -  tan"1:  2P— ]  +  v6(k) 

Xcpa<k) 


if  X  „   >  0 
cpa 


0't,    S 


tan"1:  — -S2S—  ]  -l80+vfl(k) 
Wk) 


<  0 
coa 


(3.24) 


A  is  an  angular  rotation  term  needed  to  give  the 
correct  sign  for  a  given  geometry.   A  is  +1  for  counter- 
clockwise rotation  about  the  sensor  and  -1  for  clockwise 

rotation . 

The    frequency    observation   equation   is 


f(k)    = 


F    (k)    •    v 
o  p 


V        + 

p 


vs(k)    •    Xcpa(k) 


£Rcpa(k)2   +   Wl0?]1/2 


+    vf(k)  (3-25) 
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The  excitation  matrix,  Q,  for  the  states  is  found  in 
a  similar  manner  to  that  for  the  X-Y  coordinate  system.   A 
heading  change  effects  both  R    and  Xcpa  while  a  velocity 
change  only  effects  X    .   The  random  excitation  will  again 
be  modeled  as  zero  mean,  piecewise-constant  random  heading- 
angle  velocity,  random  change  in  speed,  and  random  change  in 
rest  frequency.   With  the  expressions  for  the  random  forcing 
terms  included,  the  state  equations  become 


Rcpa(k+1)  =  Rcpa(k)  +  A  '  Xcpa(k)  '  Y6S  '  T 

XcPa(k+1)  "  XcPa(k)  +  vs(k)*T  +  t  ^/   "  A,RePa(k)  "K    '  T 


v  (k+1)  =  v  (k)  +  y-   •  T  (3-26) 

s         s       vg 


e(k+i)  =  e_(k)  +  yx     •  t 

s         s       «s 


P  (k+1)  =  PQ(k)  +  yp     •   T 


The  Q  matrix  can  thus  be  found  and  is 
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X       2-T2.a-   2  !  R      X      T2a-   2 
cpa            0  cpa  cpa      0 

S        I  S 

i 


Q(k) 


°     i  -^wV1  o 

i  s    i 


-r 


S  SI  S  S 

1 J 1     - 

2       2  ! 

T^a*   *  0  0 

v  I 

1   _L 


Symmetric 


m2  2 

o 


(3.27) 

Here  only  the  first  two  rows  involve  state  related  terms. 

The  nonlinear  observation  expressions  are  obtained 
the  same  way  as  in  the  X-Y  filter.   The  resulting  matrix  is 


H(k)  = 


80(k)     90(k) 


3R 


cpa 


3f(k) 

3R 
cpa 


3X 


cpa 


3X 


cpa 


36(k) 

3v„ 


3v 


3000 

3  6' 


30 


36(k) 

3F 
o 


3f(k)     3f(k)     3f(k)     3f(k) 


3P. 


(3-28) 
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H(k)    = 


-A-Xcpa(k) 
R'(k)2 


f'(k)^-v^(k) 
F^(k)vp 


Rcpa(k)>Xcpa(k) 
R'(k)3 


-A-R1      (k) 
R'(k)2 


f'(k)2-v'(k) 


F;(k)vp 


._  i 


R'     c 

cpa 

R'(k): 


f'(k)' 


P(5(k)vp 


xcPa(k> 

R'(k) 


f'(k) 
"FjTkT 


(3.29) 


where    f ' (k)    is   the   predicted   frequency  measurement   and 


R'OO   -    LR;pa(k)2  +  x;pa(k)2]1/2        , 


(3-30) 


is  the  predicted  range. 

The  $,  Q,  H,  and  R  matrices  are  again  substituted  in 
the  Kalman  filter  equations  at  each  time  point. 


B.   SINGLE  SENSOR  INITIALIZATION 

For  a  deterministic  problem,  since  there  are  five 
unknowns,  a.  set  of  five  independent  measurements  or  equations 
are  necessary.   With  a  sensor  which  measures  signal  arrival 
angle  and  signal  frequency,  this  would  require  measurements 
at  three  different  times  to  give  six  equations.   Because  of 
the  transcendental  functions  in  the  doppler  expression, 


these  equations  cannot  be  solved  in  closed  form  to  give 
the  unknowns,  and  some  type  of  numerical  technique  for  the 
solution  of  nonlinear  equations  must  be  used.   The  addition 
of  the  measurement  noise  complicates  the  situation  further, 
especially  for  the  angle  measurement  since  the  angle  noise 
may  be  large . 

To  reduce  the  effect  of  noise,  the  difference  in  the 
angle  measurements  at  different  time  points  should  be  as 
large  as  possible.   Appendix  C  shows  the  probability  of 
having  a  given  error  in  the  angle  difference  measurements 
as  a  function  of  the  measured  angle  difference,  with  the 
measured  difference  in  standard  deviations  of  the  noise. 
Figure  CI  shows  the  larger  the  difference  relative  to  the 
expected  value  of  angle  noise,  the  more  accurate  will  the 
measurement  solution  be.   This  may  be  difficult  to  obtain 
for  three  time  points  in  a  practical  situation,  since  the 
target  may  not  be  held  for  a  long  enough  time  to  show  a 
large  angle  change  due  to  fading  signals  or  convergence  zone 
propagation. 

There  is  an  alternate  method  whereby  an  independent 
measurement  can  be  obtained  by  looking  at  the  rate  of  change 
of  the  doppler  shifted  frequency  with  time.   The  rate  of 
change  can  be  approximated  with  finite  difference  expressions 
and,  because  of  range  dependence  in  the  derivative,  the  range 
can  be  calculated  with  measurements  at  only  two  different 
times.   Furthermore  the  heading  of  the  target  can  be  found 
if  the  speed  of  the  target  is  assumed  known.   The  condition 


is  usually  known  to  sufficient  accuracy  to  give  a  good 
heading  estimate. 

The  Doppler  equation  is 


P 
f  =  — .  (3.3D 

i  +  — -  cos(e  -e) 
p 


where  f  is  the  doppler  shifted  frequency,  F   is  the  rest 

frequency,  v   is  the  target  speed,  v   is  the  velocity  of 
s  p 

propagation  of  the  signal,  6   is  the  target  heading,  and 

s 

G  is  the  signal  angle  cf  arrival. 
Taking  the  derivative  gives 


f§  = ^-!fi .  (_!£sln(es-e).(-§))  ,  (3.32) 

[i  +  ~  cos(e  -e)r     p 

VP     s 


which  reduces  to 


§  -  -  jfiL-  •  vs  sincere)  •  §     .  (3.33) 


The  transverse  velocity  about  the  sensor  is 


Ro  fl  =  vs  sin(es-6)    ,  (3.31) 


with  R  the  target  range,  which  when  substituted  into  the 


previous  equation  gives 
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dt  "     f~v~  •  Vdt}  •  (3-35) 


Solving  for  the  range  R  yields 

Ro  "  ~  ^  '  I  '  — ^-T   •   •  (3'36) 


Substituting  finite  differences  for  the  derivatives  gives 


P  -v  -Af-At 

RQ  *  -  -V2— ? •  (3-37) 

°       f^(A0)^ 


Since  FQ/f  *  1   this  gives 

v  -Af-At 

R  s  -  -2 5-   •  (3.38) 

°      f.(A6)2 


Equation  (3.33)  can  be  solved  for  the  heading  by  writing 


F  -v  • (df/dt) 
v^  sin(e  -6)  =  -  ——2 ,  (3.39) 

s     s      f^(de/dt) 


and  by  substituting  finite  differences  for  the  derivatives 
and  assuming  some  value  for  v  ,  the  result  is 


V  -Af 

sin(0  -0)  ~  -  .  P  „    .  (3.40) 

s     ~     f* A8  *v 


r 


3 


There  are  two  solutions  to  this  equation  since 
sin(6)  =  sin(l80-9).   These  two  solutions  correspond  to 
the  up  and  down  doppler  case,  that  is 


es  up  =  180  +  e  -  sin"1!!-  f^eTT-1  C3.4ia) 

s 

and 

v  -Af 

es  down  =  e  +  sln "  C-  f.ge-v  ]    •  (3-1,lb) 


The  exact  difference  equation  solutions  are  presented 
in  Appendix  A.   The  derivatives  above  have  been  approximated 
at  the  angle  midway  between  the  two  data  points.   Figure  3.3 
shows  the  geometry  for  the  calculation.   Note  that  the  range 
can  be  found  independently  of  velocity.   These  four  quantities , 
the  target  range  and  target  heading,  the  target  bearing  and 
assumed  velocity  are  sufficient  to  initialize  the  extended 
Kalman  filters  because  all  of  the  initial  states  can  be 
calculated  from  them. 

The  first  quantity  to  be  calculated  is  the  initial  target- 
heading  G  T,  from  the  equation  (3.4la)  or  (3.'!lb).   The  value 
of  the  angle  difference,A6,  comes  from  the  two  angle  observa- 
tions which  satisfy  the  criterjon  of  being  greater  than  three 
standard  deviations  of  the  measurement  noise  apart.   With 
this  condition  and  assuming  normally  distributed  measurement 
errors,  Appendix  C  shows  that  the  probability  that  the 
fractional  error  in  the  angle  measurement,  0  /A0,  where  0 
is  the  measurement  error,  and  A0  is  the  measured  angle 
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Fig.   3.3     Calculation  of  Initial   Range,  Heading,  and 
Rest  Frequency 
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difference,  is  less  than  .5  is  equal  to  .7.   This  criterion 

was  somewhat  arbitrary  but  was  found  to  give  satisfactory 

results  in  most  instances.   It  represents  a  compromise 

between  good  initial  values  and  long  waiting  times  to  start 

the  filter.   Note  that  instead  of  using  only  the  endpoint 

measurements  to  calculate  the  midpoint  angle,  any  other 

measurements  of  the  angle  taken  in  that  interval  may  be  used. 

The  appropriate  weighted  average  is  used  to  obtain  the 

result.   For  periodic  measurements  this  is  just  the  simple 

average  of  the  measurements.   The  midpoint  angle  calculated 

in  this  manner  is  referred  to  as  6flVE. 

The  value  of  the  frequency  difference,  Af,  is  obtained 

by  subtracting  the  first  and  last  frequency  measurements 

obtained  when  the  angle  condition  is  satisfied.   The  quantity 

v  comes  from  the  known  propagation  velocity,  f  is  taken  as 

one  of  the  measured  frequencies,  and  v  T  is  the  initial 

'      si 

guess  at  the  target  speed.   When  the  quantity 


Sln(0sl-6AVE)  "  "  A6?f -v  ;  (3'i,2) 

si 


is  calculated  and  its  absolute  value  is  greater  than  one,  a 

correction  on  the  velocity  guess  may  be  made  to  bring 

|sin(6  T-8.Tn-,)|  less  than  one.   Geometrically  this  means 
1      si   AVE  ' 

that  for  the  quantities  measured,  the  speed  of  the  target 
was  not  large  enough  to  traverse  the  angle  measured. 
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When  this  is  done  9  T  may  be  computed  using  equation 

S  J. 

(3.^1a)  or  (3.4lb).   The  approximate  knowledge  of  the  rest 
frequency  and  the  frequency  measurements  are  usually  suffi- 
cient to  determine  whether  the  target  is  up  or  down  doppler. 
The  range  to  the  target  can  then  be  calculated  by  using 
equation  (3-38). 

The  initial  rest  frequency  can  be  calculated  by  averaging 
the  doppler  equations,  again  over  the  time  that  A8  was  being 
measured.   The  doppler  equation  is 


Tjl 

l  +  Ji  oos(esl-e1) 


where  T,    and  6.  refer  to  the  i   measurement  in  the  start-up 
interval.   P  T  is  the  initial  rest  frequency  to  be  calculated. 
Solving  for  the  initial  rest  frequency  F  _  gives 


FoI  =  fi[1  +  v^cos(esl-0i^    >  (3-41° 

ir 


and  averaging  over  the  initial  KJ  measurements  gives 


i   KJ         vqT 
PoI  -£  I   f.[l+^cos(esI-9i)]   ,      (3.^5) 

J.   J.  t/ 


which  is,  for  small  angle  differences  between  angle  measure- 
ments, approximately 


v 

r 
P 


PoI  .  PAVEC1  +  ^  cos(OsI-oAVE)]   .        (3.46) 
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Once  the  initial  range 5  heading  and  rest  frequency  are 
calculated,  the  initial  conditions  for  each  filter  can  be 
found.   For  the  X-Y  filter  the  equations  for  the  initial 
filter  states  are 


X10  =  xo  =  Ro  cos  GAVE 


XOA  =  V  ^  =  V  T  COS  6  T 

20    xo    si      si 


x30  =  ^0  =  Ro  sln  6AVE 


x40  =  Vyo  =  vsl  Sin  6sl 


x50  =  FoI  (3^7) 


For  the  R   -X    filter  the  initial  states  are 
cpa   cpa 


x10  =  Rcpal  =  A-Rosin(6sI-eAVE) 


X20  =  Xcpal  =  Rocos(esl-0AVE) 


X30  =  Vsl 

X40  =  6sl 

x50  -  FQI  •  (3.48) 


where  A,  as  before  is  the  rotation  parameter. 

The  initial  states  which  are  calculated  by  this  technique 
are  the  estimated  state  values  at  a  time  approximately 
midway  between  the  two  end  measurement  times  and  at  an  ane;le 
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equal  to  8.VE<   The  state  equations  are  now  used  to  bring 
the  states  back  to  the  first  measurement  f,  and  9,  and 
filtering  begins  with  this  value.   In  this  manner  all  of  the 
angle  and  frequency  measurements  that  were  not  used  in  the 
initialization  phase  can  now  be  processed.   These  would 
correspond  to  the  time  points  t+T  and  t  +  2T  in  Figure  3.3. 

One  result  of  this  initialization  technique  is  to  corre- 
late the  measurements  in  the  interval  with  those  states 

which  are  a  .direct  function  of  8.,^  or  PATn-,.   This  violates 

AVE      AVE 

the  assumption  usually  made  for  Kalman  type  filters  which 
is  that  all  measurement  noises  are  uncorrelated  with  the 
initial  filter  states. 

This  effect  decreases  as  the  number  of  points,  K  J ,  in 
the  initialization  interval  increases.   This  is  due  to  the 
averaging  that  was  used  to  calculate  the  midpoint  angle  and 
frequency.   Consider  the  covariance  between  an  angle,  Q.,    in 
the  interval,  and  the  average  over  that  interval,  8.™, 
which  is  used  for  position  determination.   The  covariance  is 

cov[e.,eAVE]  =  E[(91-elo).(eAVE-eAVEo)],  (3.49) 


where  8.   and  8.™   are  the  mean  values.   Writing  out  the 
10       AVEo 

terms  gives 


COv(0i'6AVE)  =  E^(0i-°io) 

.W  •  '  -^jl  +  Okj    V^O*"  •  +  01Q-'----  +  0KJOn 
•(        KJ  ~  KJ  ;J 

(3.50) 
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which  can  be  rearranged  to  get 


cov(e19eAVE)  =  E[(01-elo) 


rei-910  ,  92-e20  .    ,  9i-6iO  ,    ,  6KJ-9KJ0  ■■ 

(3.5D 


Thus,  the  covariance  is 

cov(eiSeAVE)  =  -^   ,  C3.52) 

2 

with  a.   the  angle  noise  variance.   The  same  results  apply 

to  the  frequency  measurements  and  F.,„  which  is  used  to 

calculate  the  initial  F  T. 

ol 

The  other  requirement  to  initialize  the  filter  is  the 
initial  value  of  the  covariance  matrix.   Since  all  of  the 
initial  states  for  each  filter  were  calculated  from  the  same 
set  of  measurements,  the  initial  filter  states  will  be 
correlated  with  each  other.   This  correlation  shows  up  as 
cross  terms  in  the  calculated  initial  covariance  matrix  and 
represents  the  relationship  between  the  initial  data  and 
the  calculation  of  the  initial  states.   Put  another  way, 
the  cross  terms  help  describe  the  region  in  state  space 
where  the  true  initial  states  are  most  likely  to  be  as 
specified  by  the  given  measurements  and  the  method  of 
initial  state  calculation. 
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C.   MULTIPLE  SENSORS  -  FREQUENCY  ONLY 
1.   X-Y  Filter 

To  add  additional  sensors  to  the  filter,  the 
observation  equations  at  the  new  sensor  must  be  found. 
Thus,  in  equations  (3.l8a)  and  (3.l8b),  the  predicted  x  and 
y  distances  to  the  new  sensor  were  used  in  place  of  the 
predicted  states  x  and  y.   If  the  new  sensor  is  at  x^y^ 
the  predicted  distances  are 


XB   x'    XB 

yB  =  y'  "  yB  (3.53) 


The  differentiation  is  then  performed  and  evaluated  at 
the  predicted  state  values  to  give  additional  rows  to  the 
H  matrix. 

With  an  additional  sensor  of  frequency  only  a 
different  Initialization  scheme  was  used.   The  rest  frequency 
and  target  heading,  midpoint  angle  and  approximate  range, 
were  obtained  using  the  single  sensor  equation.   From  the 
doppler  equation  for  the  second  sensor  an  average  angle 
from  the  sensor  to  the  target  can  be  approximated. 

Let  0.  be  the  unknown  angles  to  the  target  from  the 
second  sensor,  measured  during  the  initial  measurements  on 
the  first  sensor.   Thus  I  goes  from  1  to  KJ.   The  frequencies 
f..  are  the  observed  quantities.   Thus 
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F  T  v 
f,  =  ^— ^ .-  (3.54) 


v  +  vsIcos(eSI-ei) 


KJ  frequency  measurements  will  be  made  from  the 
second  sensor  and  it  will  be  assumed  that  they  are  made  at 
the  same  time  as  the  measurements  on  the  first  sensor. 

Adding  the  above  KJ  equations  and  solving  for  the 
cosine  cosine  terms  gives 


KJ  v_     KJ    PoT 

S  cos(eoT-e, )  =  -2_  •  T.     (  -g±  -  1)   .       (3.55) 
i=l      s±  1    vsl   i=l    T± 


Let 

,   KJ 
)  =  T^r   E   cos(6oT-0.) 


and 


cos(esI-e2AVE)  =  m    e  cos(esI-e.. 


p  t     i   KJ   F  - 

Ol       _    JL_      y  01 


F2AVE    KJ  1=1   fi 


Therefore , 


eP,w  =  eoT  ±  cos-1[  -E-  (^-^  -  D]   .       (3.56) 
2AVE    si  vsI  P2AVE 


As  illustrated  in  Fig.  3-^,  there  are  two  solutions 
for  the  angle  from  the  sensor  to  the  target.   Each  of  these 
can  be  used  with  the  average  angle  over  the  same  time 
interval  measured  from  the  first  sensor.   The  solution  to 


G? 


e 


2  AVE  "  Dsl 


u.,  -   ±  cos-1C^_  (^-  -  1)] 
vsl  hAVE 


d-csc(<}>-  eAVE) 

R   =  . 

ctn(<j)-eAVE)  -  ctn(<j)-e2  AVEi) 


ANGLE-FREQ 
SENSOR 


POSITION  USING  ONLY 
FIRST  SENSOR 


Fig.  3.4  Initial  Range  with  Second  Frequency-Only  Sensor 


this  trlangulation  problem  which  is  closest  to  the  single 

sensor  range  is  then  used  as  the  initial  range  for  the 

filter. 

The  reason  the  second  sensor  is  used  at  all  in  the 

initialization  is  to  insure  that  the  measurements  of 

frequency  of  the  second  sensor  will  be  reasonably  close  to 

the  predicted  values  that  the  filter  will  obtain  for  its 

first  few  samples.   If  this  is  not  done,  the  predicted 

values  may  be  so  far  off  that  the  linear  expansion  of  the 

frequency  term  for  the  second  sensor  used  for  the  H  matrix 

is  invalid,  and  the  residual  will  be  so  large  that  divergence 

occurs . 

2.   R  „-X   -  Filter 
cpa   cpa 

With  the  R   -X    filter  it  is  more  complicated  to 
cpa  cpa  ^ 

add  additional  sensors  because  the  states  were  functions  of 

the  first  sensor  only  and  not  of  the  coordinate  system  as 

is  the  case  for  the  X-Y  filter.   The  derivatives  in  the 

observation  linearization  must  be  taken  with  respect  to  the 

states  of  the  first  buoy.   Equations  (3.24)  and  (3.25)  for 

the  observed  frequency  and  bearing  angle  at  the  sensor  are 

general  equations  if  R   „  and  X  nn    are  referenced  to  that 
^  cpa      cpa 

sensor.   As  shown  in  Figure  3-5>  R  _.Q   and  X     for  a 

second  sensor  can  be  written  in  terms  of  the  R   „  and  X  ^„ 

cpa      cpa 

of  the  first  sensor,  which  are  states  of  the  filter.   In 
these  expressions  the  angular  rotation  parameter  A,  the 
distance  to  the  second  sensor,  d,  the  bearing  angle  to  the 


6k 


Fig.  3.5  Geometry  of  2nd  Sensor  for   R       —X 

cpa        cpa 

Filter. 


05 


second  sensor  <f>,  and  the  target  heading,  6  ,  are  all 

s 

significant  factors.   The  result  is 


R  =   R  +  A-d-sin(4>-6    ) 

cpa~  cpa  r      s 


X  =   X     „   -   d   cos(<f>-9    )  (3.57) 

cpa9  cpa  Y     s  \j'si / 


Note  that  this  calculation  could  result  in  a 
negative  R    ;  however,  this  indicates  that  the  angular 
rotation  about  the  second  sensor  is  different  from  the 
first  sensor. 

The  resulting  observation  equation  for  the  second 
sensor  is 


F  (k)-v 

fp(k)  =  - 2 ,   (3.58) 

vs(k)-(Xcpa  -  d.eos(<j,-es(k))) 

vp  +  R^Tk)        '' 

where, 

R~(k)2  =  (R„„a(k)  +  A-d-sin(<f>-9  (k)))2 

+  (X„(k)  -  d-cos(cf>-e  (k)))2   ,  (3-59) 


Also, 


A«(R  a(k)+A'd-sin(4>-e-(k))) 
G?(k)  =  e_(k)  -  arctan  ( SES g )    < 

s  X      d-cos((j)-e  (k)) 

cpa  s       (3.60) 
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There  is  an  additional  term  of  -180°  in  the  last  equati 


on 


if  X  „„  is  less  than  zero,  that  is,  before  cpa.   These  two 
cpa 

equations  are  then  differentiated  with  respect  to  the  filter 
states  to  give  the  new  rows  to  the  observation  matrix  H. 
The  result,  omitting  the  time  argument,  is 


9f2 

8x~  =  [f21   f22   f23   f?.'4        f 25  ]         (3.61) 


with 


f 


9f2   .  f22vs(Xcpa-d-COs(*-es))(RcPa+A'd-sin(*-es^ 


21    9R  -p    „  3 

cpa  Fo  vpR^ 


,   _  !£2_  _   f22vs(Rcpa+A-d-sl^^-9s^ 
22    9XcPa  Po  vp  R23 


5f2       f2^cpa-d-cos(^es^ 

o  p   2 


9f2     f22  vs  d 

*?ii  =  a*A~~   =  t  sin((j»-e  ) 

30s      Fo  vp  R2 


(3.62) 


(3.63) 


(3.64) 


(xcpa-d-cos(»-es))(A.Rcpacos(»-eB)+xcpasin(»-es) 

2 
R2 

(3.65) 
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25 


9Fo  '  Fo 


(3.66) 


and 


9e2 

3T"  =  [621    622 


623    624 


e25]  , 


(3.67) 


with 


86. 


21    9R 


cpa 


A(X   -d-cos(4>-ec,)) 
cpa o 


R, 


(3.68) 


962     A(R   +A-d-sin(<J)-es)) 


22    9X 


cpa 


R, 


(3.69) 


9.6, 

'23  =  wc 


=  0 


(3.70) 


'24    9 


™2   =    1   +    d(Xcpa.CQS(^es)-A-Rcpa.sin(^-9s)-d) 


R, 


(3.7D 


96, 

'25  "  9F~ 


(3-72) 
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IV.   DIRECT  METHOD  OF  COVARIANCE  CALCULATION 

In  Appendix  B  a  method  of  calculating  variances  for 
functions  of  independent  random  variables  was  extended  to 
include  the  calculation  of  covariances  as  well .   This 
technique  was  used  in  order  to  calculate  the  Initial 
covariance  matrix  for  the  two  types  of  filters .   The 
resulting  values  were  in  some  cases  an  order  of  magnitude 
larger  than  one  would  expect  based  on  the  accuracies  of 
the  measurements  and  the  geometry  of  the  problem. 

The  reason  for  this  is  the  highly  nonlinear  nature  of 
the  functions  involved  in  the  initial  condition  calculation. 
The  computation  of  the  heading  angle,  for  example,  consists 
of  the  arcsine  function  whose  derivative  approaches  infinity 
as  the  argument  approaches  plus  or  minus  one.   In  reality  the 
range  of  values  for  the  arcsine  function  is  bounded  by  plus 
or  minus  ninety  degrees  which  represents  the  target  being 
at  cpa.   Thus  when  one  uses  the  derivative  formulation  the 
variance  or  approximate  error  appears  much  larger  than  it 
really  is . 

A.   INITIAL  COVARIANCE  MATRIX 

An  alternate  approach  was  developed  which  more  accurately 
reflects  the  error  variances  of  the  initial  filter  conditions. 
The  measurements  Af,  A6,  6AVF>  and  the  initial  speed  guess  v  , , 
are  considered  to  be  Gaussian  random  variables  with  known 
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standard  deviations  based  on  the  measurement  noise  and  the 
assumed  speed  accuracy.   The  changes  that  occur  in  the 
calculated  Initial  conditions  when  each  of  the  measurements 
changes  by  plus  or  minus  one  standard  deviation,  are 
measures  of  the  extent  over  which  the  initial  conditions 
will  vary  and  thus  are  an  indication  of  the  accuracy  of  the 
calculation.   These  changes  can  be  calculated  using  the 
boundary  conditions  imposed  by  the  geometry  of  the  problem 
and  are  not  restricted  to  a  linear  region  or  a  smooth 
function  as  required  in  the  derivative  method. 

In  Pig.   4.1  is  shown  a  one-dimensional  example  of  a 
nonlinear  function  g(x) .   The  tangent  line  drawn  at  x,s  is 
the  derivative  of  the  function  at  x.  *  and  its  extension  by 
one  standard  deviation  shows  the  magnitude  of  the  error 
computed  by  the  derivative  method.   Also  shown  is  the  direct 
calculation  of  the  changes  in  g(x,  )  when  x,*  is  changed  by 
one  standard  deviation.   For  this  case,  the  errors  one  might 
expect  are  much  larger  than  the  derivative  calculation 
indicates.   The  difference  of  this  up  and  down  direct 
calculation,  divided  by  two,  is  therefore  taken  as  an 
equivalent  "standard  deviation"  for  the  calculated  value. 
This  is  done  for  each  of  the  three  initial  measurements  and 
v  j.   While  no  implication  is  made  that  this  value  represents 
the  true  standard  deviation  of  the  particular  initial 
condition,  it  does  represent,  for  monotonic  functions,  the 
range  of  values  for  a  one  standard  deviation  change  in  the 
measurements.   This  in  turn  is  a  measure  of  the  accuracy  of 
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g(xk+o-xk). 

^<rXk        g(X*) 
dXk     K  -     k 


X 


* 


xk+% 


5q  ...     0(X*+o-x    )-g(Xk-<rx 

Replace    —-  crY     with  k — — ! 

axk    Xk  Z 


>XL, 


Fig.  4.1  Taylor  Series  Expansion   Versus    Direct    Calculation 
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of  the  calculation  and  indeed  this  is  what  is  ultimately 

needed  for  the  initial  covariance  matrix.   The  off -diagonal 

terms  in  this  matrix  can  also  be  calculated  using  the  method 

of  Appendix  B  with  the  derivative  times  the  measurement 

variances  replaced  by  the  calculated  "standard  deviations" 

of  the  initial  conditions  in  equation  (B-20).   The  sign  of 

the  covariance  terms  will  be  preserved  by  considering  only 

positive  changes  in  the  measurements .   An  example  for  the 

R   -X    filter  illustrates  the  procedure.   Prom  equation 
cpa   cpa 

(3.^8)  the  initial  range  to  cpa  from  the  sensor  is 


R   oT  =  A-R  -sin(6  T-e.,„)    ,  (A.l) 

cpal      o      si   AVE     ' 


which  from  equations  (3-38)  and  (3.^2)  gives 


Af.T-v       Af-v 

R    _  =  A-( 5— £)•( B— )    .        (4.?) 

cpct±         AS  -f       AG-f.v  T 

si 


Rearranging  terms  yields 


T-v  T        Af,v_    r> 


which  can  be  expressed  as 


T1  •  v 

Rcpal  *  A   •  TT  •   sln2(esr5AVE)      •  "•'l) 


Written  in  this  form  the  boundary  condition  on  the  value  of 

R   T  is  evident  since  the  sine  term  is  never  greater  than 
cpal  to 


one . 


The  equivalent  standard  deviation  in  R   T  due  to  a  one- 

cpal 

standard  deviation  change  in  AB,  for  example,  is 


°RcpaI(A0)  "  lCRcpaI(Ae+^Ae)  "  ^pal^^Ae5  ]  '  iH '5) 

In  this  case  the  boundary  condition  is  imposed  in  the 
calculation  of  the  sine  term  in  equation  (4.4)  above  and 

T'Vgl 

the  maximum  value  for  the  range  is  — g-g —  .   Thus 


lsin(eRT-6AVF.>l  = 


Af -v. 


P- 


(Ae-aAe).f.vsI 


< 1  .      (4.6) 


This  calculation  is  done  for  each  of  the  measurements  and 
for  v  _.   The  total  equivalent  "variance",  since  the 
measurements  are  independent,  is  thus 


aR    2  =  aR    2(A6)  +  aR    2(Af)  +  cR    2(vsI)    (4.7) 
cpal      cpal  cpal  cpal 


Similarly,  the  equation  for  X„„T,  equation  (3.38),  the 

cp  a_L 

distance  to  or  from  cpa  can  be  used  to  calculate  Its 
equivalent  "standard  deviation".   Since 


XcPaI   "   Vcob(8bI-W         '  (iK8) 
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R  can  be  substituted  from  equation  (3-38)  to  give 

Xcpal   "  "AIT1   '    sln(esI-eAVE}    '    COB<eBl-W-         ('-9) 


Combining  the  trigonometric  terms  yields 


TvvsI        sin   2(6sI-6AVE) 


lcpal  A6 


(4,10) 


In  this  instance  the  geometric  boundary  is  Imposed  on  the 
calculation  of  the  angle  6  T  -  6.,ra  and  restricts  X   T  to  be 

S  -i~  AVlj  CpajL 

T  •  v^ T 
no  greater  than  the  maximum  range  allowed  which  is  — '-  ■  -  . 

AG 

The  equivalent  "standard  deviation"  can  be  calculated 
as  was  done  for  R   _  and  the  equivalent  covariance  found  by 
an  extension  of  equation  (B17)  .   This  results  in 


cov[RcpaI'XcpaI]   =   °R  <Ae)'aX        T(A9)    + 

^  ^  cpal  cpal 


aR    (Af)-oY    (Af)  +  aR    (v_T)-  Y    (v_T)  . 

n -r  A -r  K  _   t-    SI      A      -r    SX 

cpal       cpal         cpal        cpal 

(4.11) 

The  entire  initial  covariance  of  estimation  error  matrix 
can  be  filled  out  in  this  manner. 

To  demonstrate  the  differences  in  the  two  error 
estimates,  Table  II  contains  the  initial  covariance  matrix 
for  a  computer  simulation  in  which  the  target  was  near  cpa 
when  the  filter  was  initialized.   These  values  were  calculated 


n 


using  the  partial  derivative  expansion  described  in  Appendix 

B.   The  run  was  made  using  the  R   -X    filter.   In  the 

cpa   cpa 

upper  left  Is  the  variance  of  R   j .      Next  on  the  right  is 

the  covariance  of  R   _  and  X   T,  then  the  covariance  of 

R   T  and  v  T,  etc. 
cpal      si' 

The  diagonal  terms,  which  represent  the  initial  error 
variances,  can  be  expressed  in  more  convenient  units  to 
give 

^^cpal*  =  aR   T2   =    (1^7  K  yds)2 
*  cpal 

or      aR     =  lH . 7  k  yds . 
cpal 

Similarly   oy  =   14.4  K  yds    , 

cpal 


a      =  1.68  yds/sec   , 
vsl 


afl     =  71.7° 
esl 


and         aFoI   =  2.5  Hz.  (4.12) 


The  accuracy  of  the  Initial  estimates  is  typically  much 
better  than  these  standard  deviations  indicate.   With  the 
same  set  of  data,  the  direct  calculation  described  previously 
was  used  to  calculate  the  initial  covariance  of  error  matrix 
and  the  result  is  presented  in  Table  III.   The  standard 
deviations  for  the  diagonal  terms  are  now 
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r 


216.96'  184,20  -5.73  -17.07  33.80 

184.20  207.28  -16.43  -17.83  36.37 

-5.73  -16.43  2.85  1.21  -2.63 

-17.07  -17.83  1.21  1.56  -3.16 

33.80  36.37  -2.64  -3.16  6.16 


TABLE  II.   Initial  covariance  matrix  using  partial 
derivative  expansion. 


78.84  30.34  -0.23  -3.89  7.20 

30. 3^  39.29  -7.84  -3.52  7.36 

-0.23  -7-84  2.85  0.60  -1.39 

-3.89  -3.52  0.60  0.34  -0.69 

7.20  7.36  -1.39  -0.69  1.43 


TABLE  III.   Initial  covariance  matrix  using  direct 
calculation . 
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aR     =  8.78  K  yds 
cpal 


ay     =  6.27  K  yds    , 
cpal 


o  =  1.68  yds/sec   , 

si 


Si 


and     op     =  1.2  Hz    .  (4.13) 

ol 


The  net  effect  of  the  partial  derivative  method  of  covariance 

matrix  calculation  is  to  deemphasize  the  initial  conditions 

in  favor  of  the  first  several  measurements.   The  second 

method  applies  a  filtering  weight  more  in  line  with  the 

accuracy  of  each  of  the  initial  states. 

Not  only  are  the  magnitudes  of  the  errors  quite  different 

but  the  orientation  of  the  error  ellipses  which  are  determined 

by  the  covariance  terms  are  different  as  well.   This  is 

illustrated  in  Fig.  k  .2    for  the  position  variables  R   T  and 

&  *  cpal 

X   -.   For  the  two  covariance  matrices  in  Tables  II  and  III 
cpal 

a  coordinate  rotation  is  applied  to  give  uncorrelated  position 

coordinates.   Figure  4. 2a  shows  the  orientation  to  be  aligned 

with  the  computed  values  of  R   T  arid  X   T.  and  not  dependent 
*  cpal      cpal' 

on  the  sensor  position.   Figure  4.25,  for  the  direct  calcula- 
tion, shows  the  error  ellipse  oriented  with  the  bearing  line 
from  the  sensor  to  the  target  at  the  initial  position.   This 
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Target   Initial 
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&>  % 


a)  Taylor   Series 
Expansion 


Sensor 


-^  x 


b)  Direct  Calcation 


Target  Initial  Position 


Fig.4.2Error   Ellipses  for  Initial  Covariance 
Calculation 
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rotation  is  believed  to  occur  because  of  the  boundary 
conditions  imposed  on  the  calculated  errors  and  represents 
a  more  precise  picture  of  the  uncertainty  regions  associated 
with  the  initial  state  values. 

B.   GENERAL  COVARIANCE  UPDATE  AND  APPLICATION 
TO  KALMAN  FILTER  EQUATIONS 

One  of  the  problems  with  Extended  Kalman  filtering  is 
that  the  linearity  assumption  used  when  the  nonlinear  terms 
are  expanded  may  not  be  valid,  particularly  in  the  early 
stages  of  filtering  when  the  initial  filter  states  are 
significantly  different  from  the  true  values.   There  may 
be  cases  where  boundary  conditions  impose  constraints  on  the 
range  of  the  nonlinear  functions  which  cannot  be  handled 
easily  using  the  Extended  Kalman  approach.   In  the  final 
analysis  the  linear  expansion  will  only  be  valid  when  the 
error  estimates  accurately  approximate  the  true  errors  in 
the  problem. 

To  obtain  these  accurate  error  estimates,  the  method 
developed  for  the  calculation  of  the  initial  covariance  of 
estimation  error  matrix  described  previously,  can  be  applied, 
In  this  development  one  additioanl  point  becomes  clear: 
the  partial  derivative  method  (Extended  Kalman),  requires 
the  linearity  condition  to  be  valid  for  a  region  in  state 
space  extending  one  standard  deviation  in  all  dimensions. 
When  this  condition  is  not  satisfied,  the  direct  change 
method  will  provide  more  accurate  approximations  to  the 
errors  . 
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Consider  a  discrete  problem  with  n  states  and  m 
observation  variables. 

x(k+l)  =  f(x(k))  +  w(k) 


z(k)  =  h(x(k))  +  v(k) 


(4.1*0 


For  an  Extended  Kalrnan  filter  the  equations  have  the  form 


x(k+l)  =  $(k)  x(k)  +  w(k) 


z(k)  =  H(k)  x(k)  +  v(k)    , 


(4.15) 


with 


8f 
•  Ck)  =  3- 


x=x(k) 


(1.16) 


and 


H(k)  = 


8h  I 

"jfx  |x=x'  (k) 


(4.17) 


The  predicted  state  covariance  matrix  is  found  from 


P'(k)  =  $(k)  P(k)  <Kk)T  +  Q(k) 


(4.18) 


where  P(k)  is  the  current  covariance  of  estimation  error 
matrix.   To  see  how  the  linear  expansion  affects  this 
calculation,  pre-f actor  and  post-factor  a  diagonal  matrix 
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of  the  standard  deviations  of  the  states  out  of  the 
covariance  matrix.   P(k)  is  given  by 


P(k)   = 


COV(XnX^) 

1     eL 


COv(X-,  Xp) 


cov(x1xn) 


n 


(4.19( 


Performing  the  pre-   and  post-factoring  yields 

cov(x,Xp) 


POO  = 


\ 


ax  ax 
xl  x2 


cov(x-,Xp) 
°x  o  x~ 

X]_    Xp 


cov(xnx„) 


1    n 


covCx^) 
°x  O 


a. 


'x2  0 


xn 


(4.20) 


or  P(k)    =     E  (k)    R(k)   T,  (k) 


(4.21) 
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£  is  the  diagonal  matrix  of  standard  deviations  and 
R  is  recognized  as  the  matrix  of  correlation  coefficients 
whose  elements  are 


[R] 


U 


covCXj^x, ) 

a  a 
xi  xj 


(4.22) 


If  the  prediction  equation  (4.18)  is  now  applied,  the 
result  is 


P'(k)  =  $(k).£(k)'R(k)'ET(k)-$T(k)  +  Q(k)     (4.23) 


The  matrix  product  $(k)»I(k)  is  given  by 


•  (k)E(k)  = 


9xn 


9f. 
9x7 


9f. 


n 


9x, 


8xn 


9f 


n 


3xn 


xn 


n 


5 

9x. 


a. 


1     *1 


9f 


9f, 

oX/-j      Xq 


9f2 

oXa      Xq 


9f 

9xn    *n 


(4.24) 
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The  elements  of  this  matrix  product,  represent  the  linear 
expansion  of  the  function  f(x(k)),  carried  over  a  one-standard- 
deviation  interval  of  each  of  the  states.   Thus  if  the  linear 
expansion  is  not  valid  over  that  interval,  inaccurate  esti- 
mates of  the  predicted  covariance  could  result. 

However,  if  each  element  in  the  $(k)E(k)  matrix, 

3f±(x) 

a    is  replaced  by 

i   l'^2'  ""  '^ix.  5  "  "  *  '  ^n      1   1 '  ^2 '  "  *  " s  ^  i  "^  x    '  '  '  '  '  ^n 

J j__ 

2 
V  (4.25) 

then,  even  though  the  function  is  nonlinear  in  the  states, 
the  values  obtained  for  use  in  the  prediction  equation  (4.23) 3 
will  be  a  better  estimate  of  the  actual  errors . 

Each  of  the  Extended  Kalman  filter  equations  (3>5a) 
through  (3-5f)  can  be  written  using  the  direct  change 
calculation.   Let  F(k)  be  the  direct  one-standard-deviation 
expansion  matrix  for  the  nonlinear  state  vector  function 
f(x(k)),  and  H(k)  be  the  direct  expansion  matrix  for  h(x(k)). 
Then  the  Extended  Kalman  filter  equations  are 


P'(k)  =  F(k-l)R(k-l)FT(k-l)  +  Q(k-l) 


-  E« (k)  R' (k)  E' (k)  (4.26) 
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G(k)  =  Z'(k)R'(k)H'T(.k)[H(k)R'(k)HT(k)  +  R(k)]"1      (4.27) 

P(k)  =  P'(k)  -  S1 (k)R' (k)HT(k) • [H(k)R ' (k)HT(k)+R(k) ]_1 

•H(k)R' (k)E' (k) 

=  [Z' (k)  -  G(k)H(k)]R'(k)Z'(k)   .  (4.28) 

Z'(k)  is  the  predicted  pre-factored  standard  deviation 
matrix  and  R'(k)  is  the  predicted  correlation  coefficient 
matrix.   The  filter  equation  for  x(k)  and  the  prediction 
equation  for  x'(k)  remain  the  same. 

Any  geometric  or  physical  state  boundaries  can  be  applied 
directly  to  the  calculation  of  equivalent  "standard  deviation" 
given  by  equation  (4.25) j  to  reduce  the  range  of  the  function 
and  thus  the  uncertainty. 

While  the  updated  values  may  not  carry  with  them 
probability  implications,  as  indeed  they  do  not  in  the 
Extended  Kalman  approach,  they  would  more  accurately 
approximate  the  errors  in  the  updating  calculations  due  to 
the  error  covariances  of  the  states.. 
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V.   COMPUTER  SIMULATION 

To  evaluate  the  two  types  of  filters  a  simulation  pro- 
gram was  written  which  computes  the  true  values  of  sensor 
measurements  at  each  sensor  for  a  specific  target  track, 
adds  independent  Gaussian  noise  values  to  these  measure- 
ments, and  supplies  the  results  for  input  to  the  two  filters 

For  the  extended  Kalman . filter  in  the  X-Y  coordinate 
system,  the  provision  was  made  to  include  up  to  two  addi- 
tional frequency-only  sensors .   The  Extended  Kalman 

R    -  X    filter  has  the  provision  for  an  additional 
cpa    cpa 

angle,  frequency  sensor,  as  well  as  up  to  two  additional 
frequency-only  sensors.   For  the  purpose  of  the  simulation 
the  rest  frequency  of  the  target  was  chosen  to  be  500  Hz. 
The  speed  of  the  target  varied  between  6  to  12  knots,  and 
a  wide  variety  of  headings  and  geometries  were  run.   These 
values  were  chosen  in  an  attempt  to  provide  realistic  tar- 
get parameters  to  the  filters.   When  using  doppler  informa- 
tion, the  higher  the  frequency,  the  more  the  doppler  shift 
and  the  better  will  be  the  frequency  measurement  for  a 
given  resolution.   At  500  Hz,  a  difference  in  speed  of  one 
knot,  provides  a  shift  in  frequency  of  approximately  .17 
Hz. 

A.   MONTE  CARLO  SIMULATION 

In  order  to  compare  the  performance  of  the  two  filters, 
and  of  each  indi vidua],  filter 'as  various  parameters  w   e 
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changed,  Monte  Carlo  simulation  runs- were  made.   The  calcu- 
lated statistics  of  these  simulation  runs  then  were  used  to 
contrast  filter  performance.   Each  run  used  input  measure- 
ments (chosen  for  convenience  to  be  equally  spaced  in  time) 
from  the  simulation  program.   After  each  Monte  Carlo  run, 
the  statistics  of  the  filter  states  at  each  time  point  were 
calculated.   The  sample  means,  x,  y,  sample  variances, 
var(x),  var(y),  and  sample  covariances  cov(x,y)  for  the 
position  variables  were  computed.   Let  the  position  outputs 

of  the  filter  at  time  t(k)  for  a  particular  run  be  x. (k) 

*  1 

and  y.(k).   The  sample  statistics  for  N  runs  are 

N  „ 
x(k)  =  ±  Z  x, (k)  (5-1) 

w  1=1 

,   N  „ 
y(k)  =  i  I  y,(k)  (5.2) 

n  i=i  x 

var[x(k)]  =  cx2  =  ^  [  Z  x.(k)2  -  Nx(k)2]    (5-3) 

vr 

var[y(k)]  =  oy2  =  ^  [  I  x.(k)2  -  Ny~(k)2]    (5.4) 


cov[x(k)y(k)]  -rxyaxay  =  ^     [  Z^ xi(k)yj (k)~Nx(k)y(k) ] 

(5.5) 


N 

Z 

1=1 


'Deutsch  [lb]  gives  the  equation  for  the  sample  variance 
which  can  be  readily  extended  for  the  sample  covariance. 
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The  choice  of  how  many  runs  to  use  in  a  Monte  Carlo 
simulation  is  always  a  difficult  one  and  must  be  a  compro- 
mise between  the  computer  time  for  the  simulation  program 
and  the  accuracy  of  the  computed  statistics.   All  the  results 
used  for  filter  comparison  in  this  work  represent  200  Monte 
Carlo  runs.   Bucy  [11]  provides  a  means  for  estimating  the 
accuracy  of  the  statistics  calculated  from  Monte  Carlo  runs 
and  his  findings  are  summarized  in  Appendix  D  and  the 
results  given  below  for  the  x  variable. 

For  200  runs  the  probability  that  the  magnitude  of  the 
difference  between  the  specified  mean,  u,  and  the  sample 
mean  will  be  less  than  ten  percent  of  one  standard  deviation 
is  .84.   That  is 

Pr  { |x  -  u|  <  .1  var(x)  }  =  .84   .  (5.6) 

The  probability  that  the  specified  standard  deviation 
will  be  within  .1  of  the  sample  standard  deviation  is 
approximately  .95.   That  is 


Pr  {.9a  <  a   <   1.1c  }  ~  .95  (5.7) 


with  c  the  specified  standard  deviation. 

B.   ROTATED  ERROR  COORDINATE  SYSTEM 

The  position  variances  and  covariances  calculated  by 
the  above  technique  indicates  that  the  errors  in  the  X  and 
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Y  position  are  highly  correlated.   Correlation  coefficients 
of  . 8  or  .9  are  typical  at  each  time  point. 

If  the  errors  were  normally  distributed  this  would 
indicate  that  there  exists  a  rotated  coordinate  system 
such  that  in  the  new  system  the  orthogonal  position  com- 
ponents are  uncorrelated.   This  is  equivalent  to  taking  the 
exponent  of  the  joint  normal  probability  density  function, 
and  applying  a  coordinate  transformation  which  eliminates 
the  cross  terms. 

The  exponent  (for  zero-mean  random  variables)  is 


2    2r  xy    2 
2LT__xy__+L.     C  (5.8) 

a  o   o  o 

x      x  y     y 


When  set  equal  to  a  constant,  this  curve,  which  is  an 
ellipse,  is  a  curve  of  constant  probability.   This  ellipse 
does  not  have  its  major  and  minor  axes  aligned  with  the 
coordinate  system  however.   By  applying  the  transformation 

x'  =  x  cos  9  +  y  sin  6  (5. 9a) 


and 


y'  =  y  cos  e  -  x  sin  6  (5.9b) 
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with 


e  =  han'^i^^f]  ,  (5.10) 

a        -a 
x     y 

the  ellipse  will  be  aligned  with  the  x',  y'  axis  and  the 
resultant  random  variables  will  be  uncorrelated.   The  new 
variances  in  this  system  are  calculated  by 

2  .    2 

a   2   ^c y_   cov(xy)  ,      v 

ax'        2     +  sin  26   '  tb.llaj 


and 

2  ,    2 
«   2  _   x     y    cov(xy)  rc-  ...  » 

ayI g sTIT^-  •  (5.11b) 


This  method  of  viewing  the  sample  statistics  provides  insight 
into  the  filter  performance  because  it  shows  the  regions  of 

high  probability  and  their  relationships  to  the  sensor.   The 

2         2 

values  of  a  ,   and  a  ,   are  referred  to  as  the  rotated  error 
x'       y» 

variances  in  the  following  section. 

C.   FILTER  PERFORMANCE 

There  are  almost  an  infinite  number  of  variables  which 
can  be  changed,  and  their  effect  on  the  filter  response 
studied.   One  of  the  objectives  of  this  research  was  to  pro- 
duce a  workable  filter  for  the  passive  location  and  tracking 
problem,  and  this  will  be  demonstrated  through  the  comparisons 
below . 
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Each  figure  is  a  computer  generated  Calcomp  plot  of  a 
simulated  track.   Unless  otherwise  stated,  a  single  sensor 
is  positioned  at  the  origin  of  the  coordinate  system.   The 
line  drawn  is  the  true  track  with  the  tic  marks  indicating 
the  observation  times,  and  the  arrow- indicating  target 
heading.   The  asterisks  are  the  Monte  Carlo  filter  points 
and  are  the  sample  average  values  of  position  at  each  time 
point  for  the  200  Monte  Carlo  runs.   Some  of  the  figures 
also  show  the  orientation  of  the  final  sample  position 
error  ellipse  by  showing  the  one  standard  deviation  major 
and  minor  axes . 

In  all  of  the  simulation  runs,  the  X-Y  filter  gave 

lower  final  error  variances  when  compared  with  the  '- 

R    -  X    filter.   For  this  reason  most  of  the  simulation 
cpa    cpa 

tests  were  run  with  the  X-Y  filter. 
1 .   Time  Between  Samples 

This  requirement  will  be  dictated  ultimately  by  the 
computational  capability  of  the  computer  which  would  be  used 
for  an  actual  system.   With  the  resolution  used  in  the  simu- 
lation set  at  .0*i  Hz,  at  least  a  25-second  time  record  is 
needed  for  processing  by  an  FPT  processor.   The  time  between 
samples  must  be  short  enough  to  allow  the  filter  to  correct 
errors,  especially  in  the  initial  phases.   If  the  time  is 
too  long  the  result  could  be  filter  divergence  or  a  poor 
estimate  of  the  states.   This  is  shown  in  Fig.  5.1  and  5.2. 
The  first  graph  Is  run  with  200  seconds  between  sampD.es  and 
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Fig.   5.1     Filter  Output  and  Target  Track  — 
POO  Seconds  between  Observations 
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Fig.   5.2     Filter  Output  and  Target  Track  - 
100  Seconds  Between  Observations 
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i      \ 


second  with  100  seconds  between  samples.   All  subsequent 
simulation  runs  were  made  with  100  seconds  between  samples. 

2 .  Effect  of  Initial  Covarlance  Matrix 

The  full  initial  covarlance  matrix,  as  described  in 
Chapter  IV,  gives  as  complete  a  picture  as  is  possible  on 
the  probabilistic  relationship  among  the  initial  states. 
For  this  reason  it  provided  better  filter  transient  response, 
and  resulted  in  lower  covarlance  of  error  values  at  the  end 
of  a  given  run.   Pig.  5-3  is  the  response  of  the  X-Y  filter 
when  only  the  diagonal  terms  of  the  initial  covarlance  were 
used  ~  a  common  practice  in  implementing  filters.   Pig.  5.^ 
is  the  X-Y  filter  response  for  the  identical  set  of  measure- 
ment data  with  the  full  initial  covariance  implemented. 
Pig.  5.5  and  5.6,  Fig.  5.7  and  5.8  and  Pig.  5.9  and  5.10 
show  similar  behavior.   The  final  rotated  error  ellipses  are 
shown  on  the  plots.   In  each  case  the  variances  were  less 
with  the  full  Matrix  than  with  only  diagonal  terms .   Figures 
5.11  and  5.12  and  Fig.  5-13  and  5.14  show  similar  behavior 

for  the  R  „„  -  X  nn    filter, 
cpa    cpa 

3 .  Error  Ellipses  and  Number  of  Sensors 

Figure  5.15  shows  the  X-Y  filter  response  for  a 
single  sensor  at  the  origin.   Figure  5.16  is  a  diagram  of 
the  one  sigma  regions  for  this  filter  when  the  rotated  error 
coordinate  technique  is  used.   Note  how  the  rotated  system 
tends  to  align  itself  with  the  bearing  angle  to  the  filter 
point.   The  rotated  error  ellipses  of  the  velocity  components 
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Diagonal -Only  Initial   Covariance  Matrix 
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Fig.   5.5     X-Y  Filter  and  Target  Track  - 

Diagonal-Only  Initial   Covariance  Matrix 
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Tig.   5.6     X-Y  Filter  and  Target  Track  - 
Full   Initial   Covariance  Matrix 
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Fig.   5.7     Filler  and  Target  Track  — 

Diagonal -Only  Initial   Co-variance  Matrix 
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Fig.   5/8     X-Y  Filter  and  Target  Track  - 
Full   Initial   Covariance  Matrix 


99 


vs     =6.0  KTS 
vsI  =  6.0  KTS 


5- 50 


Fig.   5.9     X-Y  Filter  and  Target  Track  - 

Diagonal -Only  Initial   Co-variance  Matrix 
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Fig.   5.10     X-Y  Filter  and  Target  Track 
Full   Initial  Covariance 
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Fig.   5.12     RrDa"Xcm  Filter. and  Target  Track 
Full    Initial   Covari.nce  Matrix 
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Fig.  5.13  RC0£,-X   Filter  and  Target  Track  - 

Diagonal -Only  Initial  Covariance  Matrix 
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Fig.   5.14     Rcpa-Xc       Filter  and  Target  Track 
Full   Initial   Covariance  Matrix 
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(not  shown)  tend  to  show  a  similar  although  not  as  striking 

alignment  with  the  bearing  line . 

In  Fig.  5.17  an  additional  frequency-only  sensor  was 

added  at  the  position  indicated  and  as  expected,  improved 

the  averaged  filter  performance.   The  rotated  variance  plot 

is  shown  in  Pig.  5.18  and  the  presence  of  the  second  sensor 

has  reduced  the  variance  by  a  significant  amount  in  the 

radial  direction.    This  result  will,  of  course,  depend  on 

the  sensors  and  target  geometry.   For  use  with  this  type  of 

filter,  an  additional  sensor  will  enhance  performance  when 

it  sees  a  definite  change  in  the  frequency  it  is  measuring. 

If  the  sensor  were  put  near  the  predicted  track  for  example, 

very  little  change  in  the  doppler  shifted  frequency  would 

take  place  (except  when  the  target  went  thru  CPA)  and  the 

changes  which  the  sensor  would  measure  would  be  mostly  due 

to  the  frequency  noise. 

k .      Comparison  of  the  X-Y  and  R    -  X    Filter 
1 cpa Cpa 

As  mentioned  previously  the  X-Y  filter  performed 

better  in  all  simulation  runs  than  did  the  R  ^^  -  X 

cpa    cpa 

filter.   The  Monte  Carlo  tracks  were  closer  to  the  true 
track  and  the  final  rotated  error  variances  were  lower  in 
all  cases.   Figures  5.19  through  5.24  show  a  comparison  of 
the  two  filters  for  three  particular  target  tracks.   In 
each  case  the  same  simulated  input  measurements  were  pro- 
vided to  the  X-Y  and  R    -  X    filters.   The  same  initial 

cpa    cpa 

conditions  of  position,  velocity  vnd   rest  frequency  were 
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Fig.   5.15     X-Y  Filter  and  Target  Track  - 

Single  Sensor  of  Angle  and  Frequency  at  (0,0) 
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Single  sensor  at  origin.data 
points  are  300  sec  apart 


true  track 
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Fig.  5.16  Plot  of  Major  and  Minor  Axes 
of  Rotated  Covariance  Ellipses 
for  Position.  One-Sigma  Points 
are  Marked.  Single   Sensor 
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Fig.    5.17     X-Y   Filter  and  Target  Track  —Two  Sensors; 

Angle  -   Frequency   (0,0);   Frequency   (2000, -P000) 
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Fig.  5.18   Plot  of  Major  and  Minor  Axes  of   Rotated 
Coveriance   Ellipses  for  Position.  One 
Sigma   Points  are   Marked.   2  Sensors 
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calculated  as  starting  values  for  each  filter  (expressed 

in  the  appropriate  states). 

One  possible  explanation  of  the  poorer  performance 

of  the  R    -  X    filter  lies  in  the  calculation  of  the 
cpa    cpa 

position  of  the  target  after  an  observation  has  occurred. 

For  the  X-Y  filter s  of  course 3  all  that  is  needed  are  the 

states  X  and  Y.   For  the  R  „„  -  X   „  filter  the  bearing  angle 

cpa    cpa  D 

to  the  target  must  be  found  from 


-1    A'Rrna(k) 

9(k)  =  6  (k)  -  tan  x(  a  cpa )    if  X„(k)  >  0  (5.12a) 

Xcpa(k) 

or 


-i    A'R^a(k) 

6(k)  =  G  (k)  ~  tan  x(-^-^ )  -  180   if  X„(k)  <  05 

s  x   (k)  cpa 

cpa  (5.12b) 


and  the  range  from 


R0O  =  (Rcpa(«2  ♦  Xcpa(k)2)2  .  (5.13) 


The  position  is  calculated  from  the  range  and  bearing 
Note  that  coupling  between  the  position  and  velocity  compon- 
ents has  occurred  because  the  heading  angle  appears  in  the 
bearing  angle  calculation.   Thus  the  position  calculation 
is  dependent  on  three  estimated  state  values  instead  of  the 
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Fig.   5.19     X-Y  Filter  and  Target  Track 
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Fig.   5.20     R       -X         Filter  and  Target  Track 
J  cpu     cpa 
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Fig.   5.21     X-Y  Filter  and  Target  Track 
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Fig.   5.23     X-Y   Filter  and  Target  Track 
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two  required  for  the  X-Y  filter.   A  large  noise  component 

in  the  frequency  measurement,  for  example,  which  results 

in  a  significant  change  in  the  heading,  will  thus  effect 

the  position  by  rotating  the  R    ,  X   ,  R  triangle  in 

cpa'   cpa        & 

Fig.  3.2. 

5 .   Filter  Residuals 

Poirot  [3*0  in  his  dissertation  studied  the  residuals 
of  an  estimation  process  in  order  to  determine  whether  biases 
were  present  in  the  procedure.   For  on-line  digital  filtering 
the  use  of  residuals,  the  difference  between  the  predicted 
measurement  and  the  actual  measurement,  might  be  used  for 
a  similar  goal.   In  the  simulation  program,  a  counter  was 
established  to  indicate  when  the  residuals . were  greater 
than  two  standard  deviations  for  the  angle  and  four  standard 
deviations  for  the  frequency  noise.   The  frequency  is  less 
sensitive  to  position  errors.   These  levels  were  somewhat 
arbitrary,  but  for  a  poor  track  on  a  particular  run,  the 
counter  values  were  larger  when  compared  to  a  good  track. 
The  effect  needs  further  study  but  indications  are  that 
this  could  be  used  to  reinitialize  the  filter  and  reprocess 
the  data  with  the  new  starting  values  when  the  residuals 
show  poor  performance. 
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VI.   CONCLUSIONS 

A.   PASSIVE  TRACKING  AND  NONLINEAR  FILTERING 

The  idea  of  localization  and  tracking  with  a  single 
passive  sensor  has  been  shown  to  be  feasible.   The  method 
of  calculating  the  initial  position,  heading,  and  doppler- 
rest-frequency  provides  the  key  that  enabled  the  extended 
Kalman  filter  to  perform  satisfactorily.   With  only  limited 
a-priori  knowledge,  the  filter  was  initialized  by  waiting 
for  a  sufficient  change  in  the  observations  to  calculate 
a  good  initial  condition.   The  state  vector  was  then  brought 
back  to  the  first  observation  time  to  begin  filtering. 
This  technique  enabled  the  filter  to  start  with  good  enough 
initial  conditions  so  that  lock-on  and  track  would  result. 
The  role  of  the  initial  covariance  matrix  was  shown  to 
play  an  important  part  in  the  transient  response  of  the 
filter  and  resulted  in  overall  better  performance  as  measured 
by  sample  means  and  sample  variances  in  Monte  Carlo  simula- 
tions.  That  is,  the  average  mean-square  error  at  the  end 
of  a  given  filter  run  was  less  when  the  full  initial  co- 
variance  matrix  was  used  than  if  only  the  diagonal  terms 
were  used. 

The  calculation  of  the  covariance  of  error  matrix  for 
this  nonlinear  problem  using  a  simple  Taylor  Series  expan- 
sion was  shown  to  not  adequately  reflect  the  actual  errors 
that  can  be  expected  due  to  errors  in  the  measured  quantities. 


119 


This  is  due  to  the  linear  expansion  not  being  valid  over 
a  variation  of  one  standard  deviation  in  these  quantities. 
A  method  was  developed  which  more  accurately  estimates  the 
errors  by  a  direct  calculation  of  the  changes  that  occur 
in  the  nonlinear  function  due  to  a  one -standard-deviation 
change  in  the  input  states  or  measurements.   In  this  way, 
any  physical  or  geometric  boundary  conditions  can  be  imposed 
directly.   In  an  extension  of  this  technique,  it  is  believed 
that  the  updating  of  the  covariance  matrices  used  in  the 
linear  Kalman  filter  can  be  accomplished  the  same  way  and 
the  result  will  be  a  more  accurate  weighting  of  the  measure- 
ments versus  the  predicted  filter  values. 

Even  with  a  relatively  simple  geometric  problem,  the 
choice  of  filter  states  can  play  an  important  role  in  filter 
performance.   The  choice  of  the  best  system  will  be  affected 
by  the  measurement  variables,  the  coordinate  system  chosen 
for  the  dynamic  model,  and  how  any  filter  excitation  is 
modeled. 

An  additional  sensor  measuring  frequency-only  was  found 
to  give  significantly  lower  position  error  variances  than 
a  single  angle  and  frequency  sensor.   In  general  it  was 
found  that  the  sensor  should  view  as  large  a  change  in  the 
measurement  as  possible  to  minimize  the  effect  of  measurement 
noise . 

The  residuals  of  the  filter,  that  is,  the  difference 
between  the  actual  observation  and  the  predicted  observa- 
tion, were  found  to  be  a  good. qualitative  indication  of  the 
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transient  performance  of  the  filter  for  a  particular  run. 
The  values  in  the  calculated  filter  covariance  of  error 
matrix  will  not  show  when  divergence  is  occurring.   Some 
criteria  using  the  filter  residuals  relative  to  the  measure- 
ment error  can  be  helpful  in  this  regard.   A  corrective 
action  might  be  to  reinitialize  the  filter  and  start 
processing  the  observations  over  again.   The  residuals  may 
also  be  used  as  an  indication  of  a  maneuvering  target, 
in  which  case  the  assumed  state  model  is  no  longer  valid. 

B.   SUGGESTIONS  FOR  FURTHER  STUDY 

The  most  obvious  extension  of  this  work  would  be  to 
test  the  filtering  algorithm  on  real  data.   The  capability 
of  obtaining  the  required  frequency  resolution  has  been 
demostrated  [46].   The  extreme  limits  on  the  angle  noise 
have  to  be  defined  so  that  meaningful  measures  of  the 
angle  noise  can  be  used  in  the  filter. 

The  method  of  direct  calculation  of  the  covariance 
matrices  for  nonlinear  problems  represents  a  different 
approach  in  applying  the  linear  Kalman  filter  equations, 
and  this  technique  should  be  evaluated  against  the  normal 
extended  Kalman  formulation. 

Further  study  of  the  choice  of  filter  states  and  their 
comparison  with  each  other,  is  another  area  of  interest. 
The  same  problem  considered  here  could  be  formulated  in  an 
R  -  G  coordinate  system  for  example.   In  this  system  both 
the  state  and  observation  equations  would  be  nonlinear 
however . 

121 


An  additional  area  of  study  might  include  the  effects 
of  a  maneuvering  target  on  filter  performance,  and  how  the 
choice  of  filter  states  and  their  coordinate  system  are 
involved. 
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APPENDIX  A 

Development  of  Initial  Conditions  for  Single 
Sensor  —  The  Exact  Difference  Equations 


For  the  two  dimensional  constant-speed  and  constant- 
heading  target,  there  are  five  unknowns  which  completely 
determine  the  solution.   Expressed  in  an  X-Y  coordinate 
system  these  are  position  (x,y),  the  velocity  (v  ,v  )  and 
the  rest  frequency  of  the  emitter,  F  .   The  only  a  priori 
knowledge  available  in  the  general  search  problem  is  the 
approximate  target  speed  and  an  approximate  value  for  F  . 
The  measurements  consist  of  a  noisy  angle  of  arrival  and 
a  noisy  doppler-shifted  frequency.   Using  the  change  in 
angle  and  the  change  in  frequency,  an  expression  for  target 
range  and  target  heading  can  be  found  which  is  not  sensitive 
to  knowledge  of  the  rest  frequency.   The  accuracy  of  the 
range  and  heading  calculation  can  be  controlled  by  waiting 
until  the  changes  in  angle  and  frequency  are  large  with 
respect  to  the  standard  deviations  of  the  noises  involved. 
An  expression  was  derived  from  recognizing  that  the  rate  of 
change  of  the  doppler-shifted  frequency  is  range  dependent. 
Given  range  and  speed,  the  heading  is  then  found  from  the 
angular  velocity  expression. 

From  Fig.  A-l,  at  time  t,  an  angle  6]L,  and  frequency  f1 
are  measured  and  at  time  t+AT,  62  and  fp  are  measured.   The 
doppler  equations  are 
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target 

target   course 


Sensor 


Fig.  A- 1   Geometry  of   Sensor 
and  Target. 


:\?J\ 


fl  =  Po   v »  (A"1) 


l  +  -S-  cos(e  -e, ) 

vp       SI 


and 


fn  =  F 


2    o      v 
s 


i  +  -^  cos(e  -e  )   .  (a-2) 

vp       S  d 


where  FQ  is  the  rest  frequency  and  v   is  the  velocity  of 
propagation  of  the  signal.   Time  differences  in  the  arrival 
of  the  signal  have  been  neglected  since  they  are  snail 
compared  with  the  time  scale  of  changes  in  the  frequency 
and  angle .   Subtracting  A-l  from  A-2 


Af  =  f0  -  f,  =  F  r 


2    1    oL     v  v 

l  +  -£.  cos(e„-e9)  l  +  -p-  cos(e  -e-,) 

vp       S  d  Vp       si 


fqvs      cos(es-e1)  -  cos(es-e2) 

V  V  V 

p    [l  +  -5.  cos(e  -eP)]-[i  +  -S-  cos(e_-o,)] 

V  S    d  V  SI 


F      -2  sin  |  [20s  -  (e1+62)]  •  sin  |  (Q?-^l) 
o  s 

VP  (F0/fl)  •  (F0/f2) 


Let     A8  =  $2~®~\         an<^ 


Gi  +  e2  +  °'i   +  •  •  •  +  eKJ 

yAVE  '  KJ 


l?1; 


where  6^  ,   i  =  1  -  KJ ,  are  the  angle  measurements  taken  at 

equal  time  increments  between  t  and  t+AT.   Refer  also  to 

Fig.  3.3. 
Then 


Wf2  Afi 

Af  "  "    v  Tf    '  2  Sln(es-6AVE)  '  Sin  f 

XT  ^ 


Solving  for  sin(6  -e,.,^)  yields 

S       A  Vej 


'^VW    ■   "        "'Ye .  (A-3) 

2   sin  -x-   'i\  -f0'V„ 
2  12s 


There   are   two   solutions   to  this   equation 


,  Af-F    -v 

eB-eAVE  -  sin"1  [ -£-J> ]      , 

,-,  .  fit)  n  „ 

2   sin  -=-   -f,  •f0-v„ 

2  12s 

and 


.  Af-F    -v 

180°-(G   -6flw)    =    sin-1    [ -^—2 ]      . 

S      AVE  0      ,      AG    .-      . 

2   sxn  f-L'^'Vg 


The    first    solution   is    the    down-doppler   or  receding 
target   and   the   second   solution   is   the   up-doppier   or 
approaching  target .      Thus 


Af-F    -v 

■■nm  "  °ave  +  =ta       C TT-11-  J  (A-'° 

2  Sit)         f  -fi-fj-Vg 

and 
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,        Af-F  -v 
6sup  =  180°  +  eAVE  -  sin  x[ 2_JL_ ]  .   (A_5) 

2  sin  ~  -f, -f0-vo 
2    12s 


7T   7T 

The  inverse  sine  is  taken  in  the  interval  [~pS  — ]  .   From 

the  approximate  knowledge  of  F  the  up  or  dovm-doppler  case 

can  usually  be  determined.   For  small  angles  and  since 

F 

the  expressions  simplify  to 


Vf2         f2 


1  Af'vn 

6SDWN    =    eAVE   +    Sin       [    "    AlTfT^  (A"6) 


and 


6sUP  =  6AVE  +  l8°°  "  Sln"1[  -  AeTf^V^   '  (A~7) 

2   s 


Using  the  Law  of  Sines  on  the  two  triangles  in  Fig.  A-l  gives 


a  R 


lin  ~   sin  (6-00 


and 


R 


sin  ~-       sin(eg-e2) 
Now  a+b  is  the  distance  traveled  in  time  AT  and  thus 
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a+b   =   vs-AT  =  R  sin  —   •    [ +  1 2 

sin(es-e2)      sin(es-e1) 


M        sinCe^)    +    sln(es-92) 
2         sin(6   -6.)    •    sin(Q   -6,7 


s    "2'         "-" x"s    wl 


=   R   sin   A9    -    2    sin(6s"eAVE)'COS   ¥ 


■kcos  ag  -  cos  2(es-eAVE)] 


Solving  for  R  and  substituting  for  cos  2(6  -8*,,^)  = 

S   A  VHi 

1-2  sin2(6s--6AVE) , 'and  sin  ~   cos  ~   -  |  sin  AG,  the 
result  is 


v  -AT    2  sin2(6  -eA™)  +  cos  AG  -  1 

■O      _     S  r-  S     AVE -j 

sin  AG  L         2~sinT6s-6A'^T        J 


-  Vs-AT-Sln(9s-9AVE)    n  +    cos  AG  -  1 -. 

2    sin2(Gs-GAVE)     ' 

Substituting  for  sin(6  -6AVF)  from  equation  (A-3) 

Af-vr  -F  -AT  3  A0  V_fnf5  3 

R  = 7-P— ^ [1  +  (cosAG-D-  2- sin2  ^(-A^) 2] 

2  sin  M  .sin  AG'f^^  2  AfFoVp 


Af-v  -P  -AT  p   V  -f,  -f5  ^ 


2  sin  ^y-  -sin  A0-f-L-f2  o   p 


I  :'f: 


For  typical  values  of  AG,  Af,  v  .  v_,  f ,  ,  f0   and  F  ,  the 
magnitude  of  the  second  term  is  small  compared  to  one  and 
can  be  neglected.   For  example  with 

A9  =  15° 

Af  =  .2  Hz 

f  «f 
1   2 

„  - = 500  Hz 

Fo 

vo  =  5.63  yds/sec 
s 

v  -f,  -f 
(1  -  cos  M)*  (*+—£.)*  =    .0837 
Af'Fo  vp 

Now  using  the  approximations  used  previously  for  the 
heading  calculation  and  neglecting  the  term  above 

Af-v  -AT 

R  ~  P 

(A6)^  f2 

The  error  variance  of  the  Range  and  heading  calculated 
above  can  be  determined  from  the  equations  in  Appendix  B 
and  a  knowledge  of  the  variance  of  Af,A6,  and  v  .   Let 

var(Af)  =  2cf2 

var(A8)  ■  2ae2 

var(v  )  -  ay  2 

s 
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Then  from  equation  (B-l) 


var(R)  .  (|fe)2  2a02  +  (ff^)2  2af2 


rr      2         ^2 

=  R^[  8  -%  +  2  -^  ] 
A0^      Af* 


36~    9      p     36^  p    p     98   p    p     39„  p    p 
AVE     AVE  s     s 


This  is  shown  in  Appendix  B  to  be 


2         2       2      „  2    „   2 


S    KJ    X    T2     Af2      A62    v  2 
cpal  s 


For         A6  =  15°         oQ2   -  (50)2 

Af  =  .2  Hz       af2  =  (.02  Hz)2 

vs  =  5.63  yds/sec    ay  2  =  (1>68  yds/sec)2 

s 


var(R)  =  R2(|  +  .02)   =  . 9R2  =  (3600  yd)2 


R  =  3830  yds 
var(6  )  =  .0038  +  .246[.02  +  .22  +  .04] 


=  (150)2 


1  ;u 


APPENDIX  B 

Covariance  Expressions  for  Single  Buoy 

Initial  Conditions  -  R   -X    Filter 

cpa  cpa 


The  initial  conditions  for  the  filter  are  calculated 
from  the  three  measurements  described  in  the  text:   Af,  the 
frequency  difference;  AG,  the  angle  difference;  and  v  T,  the 
assumed  target  velocity.   It  is  assumed  that  Af  and  A 6  are 
independent,  normally  distributed  random  variables  whose 
means  are  unknown  and  whose  variances  are  obtained  from 
the  measurement  accuracy.   The  variance  of  v  T  is  based  on 

o  J- 

the  a  priori  knowledge  of  target  speed  and  it  will  be 

assumed  to  be  (3  knots)  .   If  each  frequency  measurement  has 

2 

a  variance  af  ,  then  the  difference  of  two  independent 

2 
measurements  has  variance  2af  .   Likewise  the  variance  of 

2 

the  angle  difference  measurement  is  2afi  .   The  uncertainty 

due  to  the  frequency  measurement  itself  will  be  neglected 

2   2       2    2 
since  it  is  small  compared  to  other  errors,  (af  /f  << a.Q  /AG  ) 

The  angle  8„™  is  the  mean  of  the  angle  measurements  taken 
to    AVE 

before  the  filter  is  initialized 


KJ 

i=l 


6ave  =  k7A     o(V  (B-1} 


such    that 


0(tKJ)    -    0(1)    >    3-oe  (B--2) 
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The  initial  filter  conditions  are  derived  from  the 
following  expressions  given  in  Appendix  A. 

A.Af2-AT-v 

RcPaI  "  7^1 T2-  (B-3) 

si 

Af-AT-v       Af2-AT-v      , 

2  si   2 

v«t   =  V,T  (B-5) 


'   T  ■         V   T 

si      si 


Af-v 
W1^ 2_ 


9sl    =  eAVE  *  Sln   <"  " ^7"  >  -  ° 

A6'vsl'f2    +  180°     (B-6) 


PoI    =  FAVE[1  +  vfIcOs(0sI-eA^)]   '        (B~7) 


The  initial  covariance  matrix  consists  of  all  second 
order  expectations  over  the  probability  space  of  the  four 
random  quantities  discussed  earlier.   The  effect  of  other 
unknowns  (v  ,  f?)  are  assumed  small  compared  with  the  above. 

Papoulis  [32]  gives  an  approximate  expression  for  the 
mean  and  variance  of  a  smooth  function  of  two  random 
variables.   Below  an  extension  is  made  for  n-random  variables 
and  an  expression  for  the  covariance  between  two  functions  of 
these  variables  is  found.   Since  Af,  AG,  and  v  are 
considered  independent  random  variables,  and  thus  their 
covariances  are  zero,  the  expressions  can  be  simplified. 
Also  since  &AVF  is  the  sum  of  independent  random  variable 
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and  A9  is  the  difference  of  two  of  them.  6     and  A9 

5   AVE 

are  independent. 

Let  x  be  a  vector  of  independent  random  variables  and 

g(x)  and  h(x)  be  two  scalar  valued  functions  of  the  vector 

x.   It  is  assumed  that  g(x)  and  h(x)  are  sufficiently  smooth 

functions  in  the  region  of  the  mean  of  x,  x  ,  that  a  Taylor 

—  — o 

expansion  of  g(x_)  and  h(x)  about  x   can  be  made.   It  is  also 
assumed  that  the  probability  density  functions  for  x  is 
symmetrical  about  the  mean  and  sufficiently  compact  so  that 
central  moments  higher  than  the  fourth  do  not  need  to  be 
considered.   If  g(x)  is  expanded  about  its  mean,  then 


(B-8) 


Jj  or 

with  derivatives  evaluated  at  x  =  x  .   The  vector  f&   is 

— "O  9  x 

defined  by 


(1£)T  k    r 
ax' 


9  6- 
ax- 


ag; 

3x, 


ax 


n 


(B-9) 


a  ?■ 
the  matrix    [■£— - — ]   is   given  by 

3xixj 


Lax.x.J 


ax/ 


a  g 


ax2ax1 


iViL_ 
3xn3x1 


3g 


ax-,  axp 


ag 


ax, 


•       •       • 


a2 


aXlaxn 


aft 


ax. 


n 


(B-10) 
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Taking  the  expectation  of  the  above  expression  and 
remembering  that  the  density  of  x  is  symmetrical  and  that 
covariances  of  the  form  E[(x.-x.  ) (x.-x.  ) ]  =  0  because  of 
the  independence  condition,  the  following  result  is  obtained, 


2 

(g(x)]  =  g(x  )  +  \       1-E?   ov   2  (B-ll) 

~°    d        8xk^   xk 


■with 


a      2  =  E[(xk-xko)2]   .  (B-12) 


Substituting  [g(x)]   for  g(x)  above 


E[g(x)2]  =  g(x  )2  +  1    [g  -9-^  +  (If-)2]  a   2     (B-13) 

k    3x,        k      k 

k 


The  variance 


var[g(x)]   =   E[g(x)2]   -  E[g(x)]2 


St*.)2  + .  eg  *\  ♦  <fi-)2]  ox  2 

^  k  9x,  dxk  xk 


g<*j2  ♦  «(*,)  ?  ^  »T. 2  ♦  f  c  ?  ^V °,  232 


k      3xk        Ak  k      3xk" 


(B-l'l 


13') 


The  last  term  is  neglected  since  it  involves  fourth  order 
terms .   Thus 


varCg(x)]  s  Z  (ff-)2  c   2  =  Z    (ff-  av  )2  (B-15) 

k  dxk    xk    k  9xk  xk 


Similarly  the  product  g(x)"h(x)  is  expanded  about  x   to  give 


E[g(x)h(x)]  =  g(x0)h(x0)  +  |E  (x-x0)T[|f^-](x-x0)     (B-16) 


The  matrix  [k-^- ]  has  terms  of  the  form 

dX.Xj 

K        =   p-  ifh_  +  IS-   .    2*L.  +  ih_  . .  i.S_  +  h  l!fi_          (b-17) 

sij         s   3x.  x.         9x,         9x,         9x,  3x.        n   3x.x.     -*    ^   xu 


Again   using  the   independence    condition 

E[g(x)h(x)]   .   g(2o)hCx0)    ♦  £  I    [g   »        +   2  If-  &-  +  b  ^]a      2 

k  3x,  k        k  3x,  k 

(B-18) 


The  covariance  is 

cov[g(x)h(x)]  =  E[g(x)h(x)]  -  E[g(x)]E[h(x)] 

s   Jg(x  )h(x_)  +  p  2  [g  o  +  ^  vr-  sY"  '  h  — -^J 

(.  -°   -o    *  k    9xk^  .    dxk  dxk     3x^ 

B(20)h(x0)  +  |  E  [g  ^  +  h  ^fsL]    2  + 

°    "°     2   k     3xk2      3x^     Xk 

1    k  3x/   Xk    j  3x^   XJ   J 
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Neglecting  the  fourth  order  term,  the  resulting  covarlance 
Is 


cov[g(x)h(x)  =  £  (|S_).(|5_) .0   2 


k  8xk   3xk   xk 


-  Z  (  |f-a_  MJg-  a_  )  (B-20) 

k   dxk  xk  dxk  xk 


The  above  expressions  for  variance  and  covariance  were  used 
to  establish  the  initial  covariance  matrix.   Let 

2 

x-,  =  Af   with  variance  2  cf     , 

2 

x0  =  A6   with  variance  2  a„     , 

x,  =  v  T  with  estimated  accuracy  given  by  a 

D  S-L  p 

variance  term  a      . 

vsl 

2 

and     X,,  =  6.    with  variance  a0 

Hi  D 

Each  of  the  partial  derivatives  were  evaluated  at  the 
measured  value  of  Af  and  AG  and  the  assumed  value  of  v  _ . 
The  resulting  terms  are  listed  below  for  the  R„__-Xrtr._ 

Cpd.    Cp  ci 

filter: 

2         2    cv   2 

var[R    ]  =  R    2  (  4  ^_  +  9  _A|_  +  _5|_  )       (B-21) 

cpai     cpai      hfd  Ae^    v  c 
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var[XopaI]  =    (XcpaI)-2[(Xopal2  -  RcpaI2)2  2^-  ♦ 

a      2  o        2 

(Rcpal2  "  ?XcpaI^2  ~f  +  Rcpal"  "4"  3       (B.22) 

Ae  vsl 


var(v  ,)  =  a        2  (B-23) 

sl     vsl 


2   „    2      2      2   a    2 
Xcpal    Af"    Ae     vsl 


2        2       2 

,_   v    (    PAVE^sT^cpaI  v  2  rgAf   ,  CA0   , 
var(Foi)  =  i-^-^j )   [—2-  +  ~y-  + 

p   o   cpal        Af     A9 

o  2 

X    2      °v 
(_cpal_  _  1}  _sl_  -,    ^  (B_25) 

Rcpal       vsl 


where 


and 


1   KJ 
PAVE  "  KJ  .^  f(i) 


2         2         2 
o     cpal     cpal 


The  covariance  terms  are 

2  2 

rn  vi-   A'RcpaI    ry  2    f?   gAf     +    fi  ^-e— ) 

°°^RcpaI-XopaI]   -  -^-  [XcpaI      (2JT+6  aq2    )   - 

R  2    (2^+    3Z"!+!i§->]      ,  (B-26) 

cpal  Af2  AQ2  2 
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with  Aj  the  angular  rotation  parameter. 

a   2 

^^cpal^I5  =  -A'RcpaI-  -^h  <B~27> 

vsl 

R    2  a      2  a      2       °v     2 

COv[RcDaiesI]  =  "  -2ML"  (2  -4"  +  3  -4-  +  -4-)    (B-28) 

cpal  si 


rovFR  fl       1    -      AVE    Vsl    "cpal    r  2 ,      ^fif  ^A6_ 

covLRcpaI8sI]  ~   v    -R    -X       - [RcpaI    U  772~~  +   3  7ZT 

c  p   o   cpal       *       Ai       AG 

+  z^~)  -  xoPai  -^        ■     <B-29> 

Vsl  vsl 

v  --R   T2   »,  T2 

°°v<XcpaIvSl'  "  Sx  T T-  (B-30) 

H  cpal  vsI 

2  2a2 

cpal    si  2        cpal  2  A02  2 

cpal  si 

2  2 

Xr>nal2    (    ^f-+    2   -T-)    3  (B-3D 

cpai  Af^  AQ^ 

2  2        2 

rv    _   n    FAVE'RcpaI  'vsl     rY     2,aAf   ,  ?  °AG   , 

^Woi'  ■  Ra„geI.Xopal2.vp  •  C=W  ^     -,?- 

V  2  „    2  n         2  °v    2 

vsl  v    R     2  ,  aAf   ,  ^AO   .   Isl  n 


2       cpal     .  r2  2  „   2 

vsI  Af     AO      vgI 


(B-32) 


i  ;(: 


covCvsIGsI]  = 


Vsl'A'RcpaI   Si2 


Lcpal 


si 


(B-33) 


v      2-P  av     2 

cov^sIPoI^   =   „   "I     J™      CXopaI2  -   *cpaI2)   — S-|-  (B-34) 


VVXcpaI 


v. 


A  •  R    ,.  •  F    •  v 

covCe  -PoT]  -  cpal  'AVE   si  E 

sl  ol     v  -R  -X    2      cpaI 
vp  no  Acpal 


CT    2 

2    Vsl 


V. 


si 


2      2   a   2 


cpal  v  Af2 


AG"    v 


si 


(B-35  ) 
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APPENDIX  C 

The  Probabilities  Associated  with  Measurement 
Errors  for  Different  Values  of  Angle  Differences 


Assume  that  the  angle  measurements  are  Independent  samples 
consisting  of  the  true  angle  plus  additive  white  Gaussian 
noise.   That  is 


ei  =  6io  +  wi 


The  probability  density  function  of  the  w . ' s  is  zero- mean 

p 

with  variance  oa    .   Thus  each  6.  is  normally  distributed 

2 
with  mean  6.   and  variance  aa    .   The  probability  distribution 
x  o  D 

of  the  difference  between  6.  and  6.,  A0,  is  therefore 

1      J 

2 
normally  distributed  with  mean  6io-8.   anci  variance  2a  ' . 

The  measurement  error 


e£  =  (e.-ej)  -  Ce10-eJO) 


is,  as  a  result,  normally  distributed  with  zero  mean  and 

2 
variance  2cr.  .   From  the  probability  tables 
6 


Pr{|eel  <  a«o0}=  Pa 

G 

with  P  the  probability  associated  with  a  normal  random 
variable  being  within  standard  deviations  of  the  mean. 
Substituting  the  angle  variance  gives 


Pr(|0j  <a</2  c0)  -   Pa 


l'lO 


If  the  angle  difference  A6  is  greater  than  3   ,  then 


Vl»el<»J2T'  >  Pa 


This  means  that  for  example 


pr{leJ  <  -5  AG}  =  P 


where 


a  =  .  5 


or  a  =  1.06  . 

The  probability  that  a  normal  random  variable  is  within  1.06 
standard  deviations  of  its  mean  is  0.71.   Thus  for  A0  >  3aQ 
the  probability  that  0  /A 6  <  .5  is  greater  than  0.71. 
Figure  CI  shows  a  general  graph  of 


P  {|e  |  <  -a^"~  Ae}  =  p 

r  '  e '  —     b  a 


a  v  2 
with  A 9  =  baQ  ,   and     a'  =  — r — 


as  a  parameter. 


Tn 


I  2 

Angle  Difference 


3  4  5 

-  in  Sid.  Dev.  b 


Fig.  C-l.    Probability  of  Angle  Difference  Error 
for  Different  Values  of  Angle 
Difference. 
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APPENDIX  D 

The  Confidence  Interval  for  Statistics 
Calculated  using  200  Monte  Carlo  Runs 


Bucy  [11]  gives  an  approximate  method  for  estimating  the 
accuracy  of  computed  statistics  based  on  the  assumption  that 
the  samples  are  from  a  normally  distributed  random  variable. 
The  sample  mean  is  given  by  equation  (5.-1  )  and  is 


-   1   N 

x  =  £  E   x,       ,  (D-l) 

1N  i=l  x 


where  N  is  the  number  of  Monte  Carlo  runs.   The  sample 
variance  is 


?    1        N       -  ? 

V  =  NTT  *   (*!"*>       •  ^"2> 

i=l 

Let  u  and  a      be  the  true  mean  and  variance  of  the  distribution 
that  the  samples  are  from,  and  the  quantities  that  are  of 
interest.   The  sample  mean  and  variance  are  unbiased >  that  is 


E(x)  =  v  and  (D-3) 


E(Sn2)  -  a2        .  (D-4) 


The  variance  of  the  mean  estimate  is  given  by 

9          p   E(x1?)  "  lJ'°   a2 
var(x)  =  a-2   -   V/J~v)' 1—^—      ■  =   %-       (D-5) 


1'I3 


The  variance  of  the  variance  estimate  is  given  by 

?        9    P    E(x.4)  -  (a2)2 
var(S^)  =  E[Sn^  -  a2]  =  ± - (D-6) 


If  the  distribution  of  the  x. 's  are  approximately  normal } 
then 

E(x.^)  *  3(a2)2    ,  (D-7) 


and  thus 

2  2 
var(Sn2)  =  cs  22  =  2  ^J-      .  (D-8) 

n 

_       p 

Prom  the  variances  for  x  and  S   and  probability  tables,  the 

n  f  5 

_  p 

confidence  region  for  x  and  S  "  can  be  found.   Let  N  be  200 
Monte  Carlo  runs .   Then 


Pr{|x-y|  <  .1  Sn}  =  Pr{|x-V|  <  a  a-   }         (D-9) 

S 

where  a—  =  -~  *  — —  ,  and  a  is  the  number  of  standard 

deviations  in  .1  S  .   That  is 


.1  s^ 

V7F 


HN 


or 


a  =  .1  J  200   =  l.'ll'l 


l'l'l 


The  probability  that  a  normal  random  variable  is  within 
l.JJl*}  a  of  its  mean  is  .84,  which  means 


Pr{|x-y|  <  .1  Sn>  ■=■  .84   .  (D-10) 


For  the  variance 


Pr{!Sn2~a2|    <    2  aq    2>    -    -95      .  (D-ll) 


Prom  equation    (D-8) 

it  *■ 


2  a      ,?  =   2 


Substituting  this    into   equation    (D-ll)    and  writing   out   the 
absolute   value   inequality   gives 


d    r       2  n,HT       2         _    2  2    „    2  /j"       2,  nc 

Pr{-  — ■ ■  a      <   S        -  a      <  — r-  a    }      =    .95      , 

n|N  JN 


or  2 

Sn  2  V 

Pr{ <  a      < }      =    .95      . 

1   +   2JJ  1-2Jl 


With   N   =    200   this    gives 


Pr{.77^   Sn2   <   a2   <   1.39  Sn2}   =    .95      , 


or 


Pr{-88  Sn   <   o   <   1.18}   =    .95 
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LIST  OP  SYSTEM  LIBRARY  SUBROUTINES  USED 


GMPRO 

ATAN 

MINV 

ARSIN 

GAUSS 

PLOTP 

ARCOS 

SIN,  COS,  TAN 

GMTRA 

SQRT 


Matrix  Product 

Arc  Tangent 

Matrix  Inverse 

Arc  Sine 

Normal  Random  Number  Generator 

Printer  Plotting  Package 

Arc  Cosine 

Trig  Functions 

Matrix  Transpose 

Square  Root 
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LIST  OF  PROGRAM  VARIABLES  COMMON  TO  BOTH  FILTERS 


R(I,J) 


FD(I,J), 
FDDT(I,J) 


BR(,J) 

VR(I3J), 
VRD  T(I,J) 

XS(J),  YS(J) 

X(I),  Y(I) 

T(J) 

XS,  YS 

VSS 

VS 

THTA 

THTAS 

OVX 

OVY 

FREQ 

VMED,  VP 


SA,  AMA 


SF,  AMF 


IXA,  IXF 
AVEVA,  VARVA 


exact  range  at  time  T(J)  from  sensor  I 
to  target 

exact  doppler  shifted  frequency  and  rate 
of  change  of  frequency  at  time  T(J)  at 
sensor  I 

exact  bearing  angle  to  target  at  time  T(J) 
from  sensor  I 

radial  velocity  and  time  rate  of  change 

of  radial  velocity  at  time  T(J)  at  sensor  I 

exact  X,Y  position  of  target  at  time  T(J) 

X3Y  position  of  sensor  I 

time  of  observation 

Initial  X,Y  position  of  target 

exact  target  velocity  in  knots 

VSS  in  yds/sec 

exact  target  heading  in  degrees 

THTA  in  radians 

exact  target  X  velocity 

exact  target  Y  velocity 

exact  rest  frequency 

velocity  of  propagation 

standard  deviation  and  mean  for  normal 
angle  noise  generator- 
standard  deviation  and  mean  for  normal 
frequency  generator- 
starting  numbers  for  noise  generator 

computed  mean  and  variance  of  random  angle 
noise 
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NSPOT 

JJQQ 

NUM 

TINT 

M 

Z(J,N) 


AVSK 

AVS 

FO 

KJ 

ADELTH 

ADELT 

ATHALP 

ADELP 

ATHETH 

PAVE 

A  SINE 

ARHALP 
FP 


number  of  different  tracks  to  be  run 

number  of  runs  in  Monte  Carlo  simulation 

number  of  time  points 

time  interval  between  data  points 

number  of  observation  variables 

noisy  observations  at  time  T(J),  Index  N 
determines  whether  it  Is  an  angle  or 
frequency  and  will  depend  on  number  and 
type  of  sensors 

initial  guess  at  target  velocity  in  knots 

AVSK  in  yds/sec 

initial  guess  at  rest  frequency  -  used  only 
to  determine  whether  target  Is  up  or  down 
doppler 

counter  which  indicates  how  many  time  cuts 
were  needed  to  satisfy  angle  difference 
criterion 

angle  difference  which  satisfies  criterion, 
i.e.,  ADELTH  >  3SA 

time  difference  corresponding  to  angle 
difference 

time  at  which  initial  filter  states  are 
found 

frequency  difference  corresponding  to 
angle  difference 

average  of  all  angle  measurements  during 
initial  condition  calculation 

average  of  frequency  measurements  during 
initial  condition  calculation 

Arc  sine  of  angle  difference  between  the 
the  target  heading  and  target  bearing 

calculated  initial  target  range 

one  standard  deviation  of  ADELF,  equals  2   SF 


III  8 


AA 


one  standard  deviation  of  ADELTH,  equals 
SA 


W 
AVAV 

HEAD  (ADELF, 
ADELTH,  AVS, 
ATHETH ) 

HUF 

HDP 

HVA 

HDA 

HUV 

HDV 

NO 

COVK 

COV 

PHI 

PHITR 

SIGTH 

SIGVS 

SIGFO 

F(M) 

QEXIT 

HOB 

HOBTR 

GAIN 

GAINTR 

ZNOIS 


guess  at  accuracy  of  AVSK,  equals  3  KTS 

one  standard  deviation  of  ATHETH,  equals 
SA/  KJ 

function  subprogram  to  calculate  target 
heading  from  initial  measurements 


target  heading  -when  ADELF  is  increased  by  FF 

target  heading  when  ADELF  is  decreased  by  FP 

target  heading  when  ADELTH  is  increased  by  AA 

target  heading  when  ADELTH  is  decreased  by  AA 

target  heading  when  AVS  is  increased  by  W 

target  heading  when  AVS  is  decreased  by  W 

=  0  for  full  initial  covariance  matrix, 
=  1  for  only  diagonal  terms 

initial  state  covariance  matrix,  also  updated 
state  covariance  matrix 

predicted  state  covariance  matrix 

state  transition  matrix 

PHI  transposed 

2 

heading  angle  excitation  in  (rad/K  sec) 

2 
target  speed  excitation  in  ( (yds/sec )/sec ) 

2 
target  rest  frequency  excitation  (H  /Ksec) 

predicted  doppler  shifted  frequency  at  sensor  N 

state  excitation  matrix 

linearized  observation  matrix 

HOB  transposed 

Kalman  Filter  gain  matrix 

GAIN  transposed 

observation  noise  covariance  matrix 

1>I9 


XCUR 

XPRED 

XPIL 

IRE 

XMC(I,J) 

XVAR(I,J) 

XTAR(J) 
YTAR(J) 


state  vector  before  observation 

predicted  state  vector  (  =  PHI -XCUR) 

filtered  state  vector  after  observation 

residual  error  counter 

Monte  Carlo  mean  of  state  I  for  time  T(J) 

Monte  Carlo  variance  of  state  I  for  time  T(J) 
(See  RCPA  -  XCPA  filter) 

filtered  X  position  of  target  at  time  T(J) 

filtered  Y  position  of  target  at  time  T(J) 


XINT(Z) ,YINT(Z)  average  of  X  and  Y  initial  positions  for 

Monte  Carlo  runs 

RPIL(J) ,TKFIL(J)   calculated  target  range  and  bearing  from 

filtered  states  at  time  T(J) 
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LIST  OF  PROGRAM  VARIABLES  FOR  X-Y  FILTER 


X(l) 
X(2) 

x(3) 

X(il) 

X(5) 

XUF 


XDF 


SGXF 


SGVXA 


X  position 
X  velocity 

Y  position 

Y  velocity 

rest  frequency 

X  refers  to  which  state,  U  refers  to  up  or 
increase,  and  F  refers  to  which  variable  is 
changed.   F  is  frequency  term  ADELF.   Thus 
XUF  is  the  calculated  X  initial  position 
due  to  an  increase  in  ADELF  by  FF 

calculated  X  initial  position  due  to  a 
decrease  in  ADELF  by  FF 

average  of  the  change  due  to  increasing 
ADELF  and  the  change  due  bo  decreasing  ADELF 
by  AA 

average  of  the  change  due  to  increasing 
ADELTH  and  the  change  due  to  decreasing 
ADELTH  by  A A 


The  other  terms  follow  the  same  pattern 


TT 


time  interval  between  measurements 


SNEWXV,  SNEWXS,  Monte  Carlo  variances  and  standard  deviations 
SNEWYV,  SNEWYS   of  X  and  Y  positions  in  rotated  coordinate 

system 


HDING 
HI 

RESA 
RESFK 


calculated  initial  target  heading 

predicted  target  bearing  from  sensor  1  in 
radians 

angle  residual  for  sensor  1  in  radians 

frequency  residual  for  sensor  K  in  H 


I  VI 


RERROR(J) 

VSFIL(J) 

HDPIL(J) 

RTEMP 

XYVAR(I,J) 

XYVAR(Z,J) 

XYA(J) 

VXVYA(J) 


true  range  error  for  time  T(J)  i.e.  distance 
from  filtered  position  to  true  position 

computed  target  speed  from  filtered  X  and 

Y  velocities  at  time  T(J) 

computed  target  heading  from  filtered  X 
and  Y  velocities  at  time  T(J) 

computed  target  range  from  filtered  X  and 

Y  positions 

Monte  Carlo  covariance  of  X  and  Y  filter 
positions  at  time  T(J) 

Monte  Carlo  covariance  of  X  and  Y  filter 
velocities  at  time  T(J) 

angle  of  Monte  Carlo  probability  ellipse 
for  position  variances 

angle  of  Monte  Carlo  probability  ellipse 
for  velocity  variances 
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LIST  OP  PROGRAM  VARIABLES  FOR  R    ,  X     FILTER 
c  p  a  - — c  p  a 


X(l) 
X(2) 

X(3) 
X(4) 

X(5) 

REX 

XEX 

NDIFA 
NLOFA 
NBU 
W 

RCPAI 
XCPAI 


range  to  target  at  CPA  from  sensor  1 

range  of  target  to  CPA 

target  speed 

target  heading 

rest  frequency 

exact  value  of  Range  to  CPA  from  sensor  1 

exact  value  of  Range  to  CPA  from  target 

number  of  angle  and  frequency  sensors 

number  of  frequency  sensors 

total  number  of  sensors  used 

angular  rotation  parameter  around  sensor  1 
(-  +1  counterclockwise j   =  -1  clockwise) 

Initial  filter  state  X(l) 

initial  filter  state  X(2) 


Initial  covariance  terms  are  defined  in  a  similar  manner  as 
the  X-Y  filter.   Thus  RW  is  the  initial  Rpp^  due  to  an 
increase  in  Avs  by  W. 


DELTAT 
D(I) 
TH(I) 
RCPA(I) 
XCPA(I) 
RANGE (I) 
THTAX(I) 
RES  1 


time  interval  between  measurements 
distance  from  sensor  1  to  sensor  I 
bearing  from  sensor  1  to  sensor  I 
predicted  range  at  CPA  fro  sensor  I 
predicted  range  to  CPA  of  target  for  sensor  I 
predicted  range  from  target  to  sensor  I 
predicted  bearing  of  target  from  sensor  I 
angle  residual  for  sensor  1 
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RES  2 

RES  3,  RES  i| 

AVRESA 
STDRAZ 

AVRP1 
STFIZ 

ICOUNT 

IEND 


XMC(I,J), 
XMC(Z,J) 

XVAR(I,J) , 
XVAR(Z,J) 

XYVAR(I,J) 


XYVAR(Z,J) 


frequency  residual  far  sensor  1 

angle  and  frequency  residual  depending  on 
number  and  type  of  sensors 

statistical  mean  of  RESI  for  a  single  run 

statistical  standard  deviation  of  RESI  for 
a  single  run 

statistical  mean  of  RES2  for  a  single  run 

statistical  variance  of  RES2  for  a  single 
run 

number  of  times  filter  has  been  reinitialized 
due  to  residual  error  counter  IRE 

maximum  number  of  times  a  reinitialization 
will  be  tried 

Monte  Carlo  mean  of  target  X  and  Y  position 
at  time  T(J) 

Monte  Carlo  variance  of  target  X  and  Y 
position  at  time  T(J) 

Monte  Carlo  covariance  of  X  and  Y  position 
at  time  T(J) 

Monte  Carlo  covariance  of  target  speed  and 
heading  at  time  T(J) 
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Flow  Chart  for  x-Y 
Filter    Simulation  Program 


CED 


READ  IXA.IXF, 
NSPOT,JJQQ 

READ  TARGET  DATA 


A 


FIX  M.NUMJINT 

FIX  SENSOR 
POSITION 


A 


COMPUTE  EXACT 
DATA  FOR  EACH 
SENSOR 


JQ=1 


pQ-JQ+lh i  H 

L5rJ 

0 


CALL  GAUSS 
ADD  NOISE  TO 
SIMULATE  INPUT 


1 


(OPTIONAL) 


-* 


WRITE 

SIMULATED 

DATA 
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Flow  Char!  for   x-Y    Filter, 
Filter  Program 


AVSK  = 
AVSK+2 


<3- 


NO 


SET  AVSK 
SET  FO 


KJ=2 


KJ=KJ+1 


COMPUTE 

siN(es-eAVE) 
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COMPUTE  INITIAL 
HEADING,  RANGE 
AND  REST  FREQ 


COMPUTE 
INITIAL  FIL- 
|LEE_SIAI£S. 


(OPTIONAL) 


W 


JfeJWRITE   INITIAL 
STATES 


INITIALIZE  THE 
COVARIANCE 
MATRIX 


(OPTIONAL)  . 


w 


TRANSFER  STATES 
TO  FIRST  MEASURE- 
MENT TIME 


w 


WRITE  INITIAL 

COVARIANCE 

MATRIX 


LL=1 
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LL=LL+1 


0 

— » 

_XL_ 

COMPUTE 

EXCITATION 

MATRIX 


COMPUTE 

PREDICTED 

STATES 


INCREMENT 
RESIDUAL 
I  COUNTER 


COMPUTE 
PREDICTED 
COVARIANCE 
MATRIX 


COMPUTE 
OBSERVATION 
MATRIX 


COMPUTE 

FILTER 

GAINS 


I 


CALCULATE 
RESIDUALS: 
ANGLE-RESA 
FREQ-RESF 
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G> 


(T)^ 


COMPUTE 
FILTERED 

ESTIMATES 


(OPTIONAL^ 


UPDATE 
COVARIANCE 


WRITE  CURRENT 
PREDICTED  AND 
ESTIMATED 
STATES. 


YES 


(OPTIONAL) 


COMPUTE 
MONTE  CARLO 
STATISTICS 


i. 


ROTATE 
POSITION  ERR 
ELLIPSES 

T 


SL 


PLOT  TRUE 
TRACK  AND 
MONTE  CARLO 
MEAN  TRACK 


Ci™_3 


PLOT  TRUE 
TRACK  AND 
FILTER  TRACK 


-a* 


WRITE 

MONTE  CARLO 
STATISTICS 


WRITE  ANGLE 
AND  NEW 
VARIANCES 
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